diff --git a/flight/Modules/FixedWingPathFollower/fixedwingpathfollower.c b/flight/Modules/FixedWingPathFollower/fixedwingpathfollower.c
index 4e658dd57..ba22ea636 100644
--- a/flight/Modules/FixedWingPathFollower/fixedwingpathfollower.c
+++ b/flight/Modules/FixedWingPathFollower/fixedwingpathfollower.c
@@ -415,19 +415,27 @@ static uint8_t updateFixedDesiredAttitude()
climbspeedError = climbspeedDesired - velocityActual.Down;
// Error condition: wind speed is higher than maximum allowed speed. We are forced backwards!
+ fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_WIND] = 0;
if (groundspeedDesired - baroAirspeedBias <= 0 ) {
+ fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_WIND] = 1;
result = 0;
}
// Error condition: plane too slow or too fast
- if ( airspeedActual > fixedwingpathfollowerSettings.AirSpeedMax * 1.2f
- || airspeedActual < fixedwingpathfollowerSettings.AirSpeedMin * 0.8f
- ) {
+ fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_HIGHSPEED] = 0;
+ 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;
}
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
// also we cannot handle planes flying backwards, lets just wait until the nose drops
+ fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWSPEED] = 1;
return 0;
}
@@ -475,21 +483,25 @@ static uint8_t updateFixedDesiredAttitude()
stabDesired.Throttle = powerCommand;
// Error condition: plane cannot hold altitude at current speed.
+ fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWPOWER] = 0;
if (
powerCommand == fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MAX] // throttle at maximum
&& velocityActual.Down > 0 // we ARE going down
&& climbspeedDesired < 0 // we WANT to go up
&& speedError > 0 // we are too slow already
) {
+ fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWPOWER] = 1;
result = 0;
}
// Error condition: plane keeps climbing despite minimum throttle (opposite of above)
+ fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_HIGHPOWER] = 0;
if (
powerCommand == fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MIN] // throttle at minimum
&& velocityActual.Down < 0 // we ARE going up
&& climbspeedDesired > 0 // we WANT to go down
&& speedError < 0 // we are too fast already
) {
+ fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_HIGHPOWER] = 1;
result = 0;
}
@@ -531,12 +543,14 @@ static uint8_t updateFixedDesiredAttitude()
fixedwingpathfollowerSettings.PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGS_PITCHLIMIT_MAX]);
// Error condition: high speed dive
+ fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_PITCHCONTROL] = 0;
if (
pitchCommand == fixedwingpathfollowerSettings.PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGS_PITCHLIMIT_MAX] // pitch demand is full up
&& velocityActual.Down > 0 // we ARE going down
&& climbspeedDesired < 0 // we WANT to go up
&& speedError < 0 // we are too fast already
) {
+ fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_PITCHCONTROL] = 1;
result = 0;
}
diff --git a/shared/uavobjectdefinition/fixedwingpathfollowerstatus.xml b/shared/uavobjectdefinition/fixedwingpathfollowerstatus.xml
index fc16f441b..b384434ca 100644
--- a/shared/uavobjectdefinition/fixedwingpathfollowerstatus.xml
+++ b/shared/uavobjectdefinition/fixedwingpathfollowerstatus.xml
@@ -4,6 +4,7 @@
+