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

made safety checks in fixed wing path follower configurable

Browse Source
This commit is contained in:
Corvus Corax 2013-07-17 14:50:23 +02:00
parent be1eb43b72
commit bffe119214
2 changed files with 20 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
flight/modules/FixedWingPathFollower/fixedwingpathfollower.c
View File

@ -446,19 +446,19 @@ static uint8_t updateFixedDesiredAttitude()
// Error condition: plane too slow or too fast
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_HIGHSPEED] = 0;
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWSPEED] = 0;
if (indicatedAirspeedState > systemSettings.AirSpeedMax) {
if (indicatedAirspeedState > systemSettings.AirSpeedMax * fixedwingpathfollowerSettings.Safetymargins[FIXEDWINGPATHFOLLOWERSETTINGS_SAFETYMARGINS_OVERSPEED]) {
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_OVERSPEED] = 1;
result = 0;
}
if (indicatedAirspeedState > fixedwingpathfollowerSettings.HorizontalVelMax * 1.2f) {
if (indicatedAirspeedState > fixedwingpathfollowerSettings.HorizontalVelMax * fixedwingpathfollowerSettings.Safetymargins[FIXEDWINGPATHFOLLOWERSETTINGS_SAFETYMARGINS_HIGHSPEED]) {
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_HIGHSPEED] = 1;
result = 0;
}
if (indicatedAirspeedState < fixedwingpathfollowerSettings.HorizontalVelMin * 0.8f) {
if (indicatedAirspeedState < fixedwingpathfollowerSettings.HorizontalVelMin * fixedwingpathfollowerSettings.Safetymargins[FIXEDWINGPATHFOLLOWERSETTINGS_SAFETYMARGINS_LOWSPEED]) {
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWSPEED] = 1;
result = 0;
}
if (indicatedAirspeedState < systemSettings.AirSpeedMin) {
if (indicatedAirspeedState < systemSettings.AirSpeedMin * fixedwingpathfollowerSettings.Safetymargins[FIXEDWINGPATHFOLLOWERSETTINGS_SAFETYMARGINS_STALLSPEED]) {
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_STALLSPEED] = 1;
result = 0;
}
@ -508,7 +508,8 @@ static uint8_t updateFixedDesiredAttitude()
if (powerCommand >= fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MAX] && // throttle at maximum
velocityState.Down > 0 && // we ARE going down
descentspeedDesired < 0 && // we WANT to go up
airspeedError > 0) { // we are too slow already
airspeedError > 0 && // we are too slow already
fixedwingpathfollowerSettings.Safetymargins[FIXEDWINGPATHFOLLOWERSETTINGS_SAFETYMARGINS_LOWPOWER] > 0.5f) { // alarm switched on
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWPOWER] = 1;
result = 0;
}
@ -517,7 +518,8 @@ static uint8_t updateFixedDesiredAttitude()
if (powerCommand >= fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MIN] && // throttle at minimum
velocityState.Down < 0 && // we ARE going up
descentspeedDesired > 0 && // we WANT to go down
airspeedError < 0) { // we are too fast already
airspeedError < 0 && // we are too fast already
fixedwingpathfollowerSettings.Safetymargins[FIXEDWINGPATHFOLLOWERSETTINGS_SAFETYMARGINS_HIGHPOWER] > 0.5f) { // alarm switched on
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_HIGHPOWER] = 1;
result = 0;
}
@ -558,7 +560,8 @@ static uint8_t updateFixedDesiredAttitude()
if (pitchCommand >= fixedwingpathfollowerSettings.PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGS_PITCHLIMIT_MAX] && // pitch demand is full up
velocityState.Down > 0 && // we ARE going down
descentspeedDesired < 0 && // we WANT to go up
airspeedError < 0) { // we are too fast already
airspeedError < 0 && // we are too fast already
fixedwingpathfollowerSettings.Safetymargins[FIXEDWINGPATHFOLLOWERSETTINGS_SAFETYMARGINS_PITCHCONTROL] > 0.5f) { // alarm switched on
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_PITCHCONTROL] = 1;
result = 0;
}
@ -577,7 +580,9 @@ static uint8_t updateFixedDesiredAttitude()
}
// Error condition: wind speed is higher than airspeed. We are forced backwards!
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_WIND] = 0;
if (headingError > 90.0f || headingError < -90.0f) {
if ((headingError > fixedwingpathfollowerSettings.Safetymargins[FIXEDWINGPATHFOLLOWERSETTINGS_SAFETYMARGINS_WIND] ||
headingError < -fixedwingpathfollowerSettings.Safetymargins[FIXEDWINGPATHFOLLOWERSETTINGS_SAFETYMARGINS_WIND]) &&
fixedwingpathfollowerSettings.Safetymargins[FIXEDWINGPATHFOLLOWERSETTINGS_SAFETYMARGINS_HIGHPOWER] > 0.5f) { // alarm switched on
// we are flying backwards
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_WIND] = 1;
result = 0;

8
shared/uavobjectdefinition/fixedwingpathfollowersettings.xml
View File

@ -42,8 +42,14 @@
<!-- maximum allowed pitch angles and setpoint for neutral pitch -->
<field name="ThrottleLimit" units="" type="float" elements="3" elementnames="Min,Neutral,Max" defaultvalue="0.1,0.5,0.9" />
<!-- minimum and maximum allowed throttle and setpoint for cruise speed -->
<field name="Safetymargins" units="" type="float"
elementnames="Wind, Stallspeed, Lowspeed, Highspeed, Overspeed, Lowpower, Highpower, Pitchcontrol"
defaultvalue="90, 0.0, 0.5, 1.5, 0.0, 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="UpdatePeriod" units="ms" type="int32" elements="1" defaultvalue="100"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/>
<telemetryflight acked="true" updatemode="onchange" period="0"/>