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

FixedWingpathFollower: Added debug output of error indicators

Browse Source
This commit is contained in:
Corvus Corax 2012-05-28 14:46:03 +02:00
parent 32fdfe5aa5
commit 089aa54a59
2 changed files with 18 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
flight/Modules/FixedWingPathFollower/fixedwingpathfollower.c
View File

@ -415,19 +415,27 @@ static uint8_t updateFixedDesiredAttitude()
climbspeedError = climbspeedDesired - velocityActual.Down; climbspeedError = climbspeedDesired - velocityActual.Down;
// Error condition: wind speed is higher than maximum allowed speed. We are forced backwards! // Error condition: wind speed is higher than maximum allowed speed. We are forced backwards!
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_WIND] = 0;
if (groundspeedDesired - baroAirspeedBias <= 0 ) { if (groundspeedDesired - baroAirspeedBias <= 0 ) {
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_WIND] = 1;
result = 0; result = 0;
} }
// Error condition: plane too slow or too fast // Error condition: plane too slow or too fast
if ( airspeedActual > fixedwingpathfollowerSettings.AirSpeedMax * 1.2f fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_HIGHSPEED] = 0;
|| airspeedActual < fixedwingpathfollowerSettings.AirSpeedMin * 0.8f fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWSPEED] = 0;
) { if ( airspeedActual > fixedwingpathfollowerSettings.AirSpeedMax * 1.2f) {
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_HIGHSPEED] = 1;
result = 0;
}
if (airspeedActual < fixedwingpathfollowerSettings.AirSpeedMin * 0.8f) {
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWSPEED] = 1;
result = 0; result = 0;
} }
if (airspeedActual<1e-6) { if (airspeedActual<1e-6) {
// prevent division by zero, abort without controlling anything. This guidance mode is not suited for takeoff or touchdown, or handling stationary planes // prevent division by zero, abort without controlling anything. This guidance mode is not suited for takeoff or touchdown, or handling stationary planes
// also we cannot handle planes flying backwards, lets just wait until the nose drops // also we cannot handle planes flying backwards, lets just wait until the nose drops
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWSPEED] = 1;
return 0; return 0;
} }
@ -475,21 +483,25 @@ static uint8_t updateFixedDesiredAttitude()
stabDesired.Throttle = powerCommand; stabDesired.Throttle = powerCommand;
// Error condition: plane cannot hold altitude at current speed. // Error condition: plane cannot hold altitude at current speed.
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWPOWER] = 0;
if ( if (
powerCommand == fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MAX] // throttle at maximum powerCommand == fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MAX] // throttle at maximum
&& velocityActual.Down > 0 // we ARE going down && velocityActual.Down > 0 // we ARE going down
&& climbspeedDesired < 0 // we WANT to go up && climbspeedDesired < 0 // we WANT to go up
&& speedError > 0 // we are too slow already && speedError > 0 // we are too slow already
) { ) {
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWPOWER] = 1;
result = 0; result = 0;
} }
// Error condition: plane keeps climbing despite minimum throttle (opposite of above) // Error condition: plane keeps climbing despite minimum throttle (opposite of above)
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_HIGHPOWER] = 0;
if ( if (
powerCommand == fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MIN] // throttle at minimum powerCommand == fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MIN] // throttle at minimum
&& velocityActual.Down < 0 // we ARE going up && velocityActual.Down < 0 // we ARE going up
&& climbspeedDesired > 0 // we WANT to go down && climbspeedDesired > 0 // we WANT to go down
&& speedError < 0 // we are too fast already && speedError < 0 // we are too fast already
) { ) {
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_HIGHPOWER] = 1;
result = 0; result = 0;
} }
@ -531,12 +543,14 @@ static uint8_t updateFixedDesiredAttitude()
fixedwingpathfollowerSettings.PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGS_PITCHLIMIT_MAX]); fixedwingpathfollowerSettings.PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGS_PITCHLIMIT_MAX]);
// Error condition: high speed dive // Error condition: high speed dive
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_PITCHCONTROL] = 0;
if ( if (
pitchCommand == fixedwingpathfollowerSettings.PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGS_PITCHLIMIT_MAX] // pitch demand is full up pitchCommand == fixedwingpathfollowerSettings.PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGS_PITCHLIMIT_MAX] // pitch demand is full up
&& velocityActual.Down > 0 // we ARE going down && velocityActual.Down > 0 // we ARE going down
&& climbspeedDesired < 0 // we WANT to go up && climbspeedDesired < 0 // we WANT to go up
&& speedError < 0 // we are too fast already && speedError < 0 // we are too fast already
) { ) {
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_PITCHCONTROL] = 1;
result = 0; result = 0;
} }

1
shared/uavobjectdefinition/fixedwingpathfollowerstatus.xml
View File

@ -4,6 +4,7 @@
<field name="E" units="" type="float" elementnames="Course,Speed,Accel,Power" /> <field name="E" units="" type="float" elementnames="Course,Speed,Accel,Power" />
<field name="A" units="" type="float" elementnames="Course,Speed,Accel,Power" /> <field name="A" units="" type="float" elementnames="Course,Speed,Accel,Power" />
<field name="C" units="" type="float" elementnames="Course,Speed,Accel,Power" /> <field name="C" units="" type="float" elementnames="Course,Speed,Accel,Power" />
<field name="Errors" units="" type="uint8" elementnames="Wind,Lowspeed,Highspeed,Lowpower,Highpower,Pitchcontrol" />
<access gcs="readwrite" flight="readwrite"/> <access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/> <telemetrygcs acked="false" updatemode="manual" period="0"/>
<telemetryflight acked="false" updatemode="periodic" period="500"/> <telemetryflight acked="false" updatemode="periodic" period="500"/>