diff --git a/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c b/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c
index 64edbe53d..cb602536e 100644
--- a/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c
+++ b/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c
@@ -181,52 +181,50 @@ static void pathfollowerTask(__attribute__((unused)) void *parameters)
uint8_t result;
// Check the combinations of flightmode and pathdesired mode
- switch (flightStatus.FlightMode) {
- case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
- case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE:
- if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT) {
- updatePathVelocity();
- result = updateFixedDesiredAttitude();
- if (result) {
- AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
+ if (flightStatus.ControlChain.PathFollower == FLIGHTSTATUS_CONTROLCHAIN_TRUE) {
+ if (flightStatus.ControlChain.PathPlanner == FLIGHTSTATUS_CONTROLCHAIN_FALSE) {
+ if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT) {
+ updatePathVelocity();
+ result = updateFixedDesiredAttitude();
+ if (result) {
+ AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
+ } else {
+ AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING);
+ }
} else {
- AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING);
+ AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR);
}
} else {
- AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR);
- }
- break;
- case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER:
- pathStatus.UID = pathDesired.UID;
- pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS;
- switch (pathDesired.Mode) {
- case PATHDESIRED_MODE_FLYENDPOINT:
- case PATHDESIRED_MODE_FLYVECTOR:
- case PATHDESIRED_MODE_FLYCIRCLERIGHT:
- case PATHDESIRED_MODE_FLYCIRCLELEFT:
- updatePathVelocity();
- result = updateFixedDesiredAttitude();
- if (result) {
+ pathStatus.UID = pathDesired.UID;
+ pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS;
+ switch (pathDesired.Mode) {
+ case PATHDESIRED_MODE_FLYENDPOINT:
+ case PATHDESIRED_MODE_FLYVECTOR:
+ case PATHDESIRED_MODE_FLYCIRCLERIGHT:
+ case PATHDESIRED_MODE_FLYCIRCLELEFT:
+ updatePathVelocity();
+ result = updateFixedDesiredAttitude();
+ if (result) {
+ AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
+ } else {
+ pathStatus.Status = PATHSTATUS_STATUS_CRITICAL;
+ AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING);
+ }
+ break;
+ case PATHDESIRED_MODE_FIXEDATTITUDE:
+ updateFixedAttitude(pathDesired.ModeParameters);
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
- } else {
+ break;
+ case PATHDESIRED_MODE_DISARMALARM:
+ AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL);
+ break;
+ default:
pathStatus.Status = PATHSTATUS_STATUS_CRITICAL;
- AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING);
+ AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR);
+ break;
}
- break;
- case PATHDESIRED_MODE_FIXEDATTITUDE:
- updateFixedAttitude(pathDesired.ModeParameters);
- AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
- break;
- case PATHDESIRED_MODE_DISARMALARM:
- AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL);
- break;
- default:
- pathStatus.Status = PATHSTATUS_STATUS_CRITICAL;
- AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR);
- break;
}
- break;
- default:
+ } else {
// Be cleaner and get rid of global variables
northVelIntegral = 0;
eastVelIntegral = 0;
@@ -234,8 +232,6 @@ static void pathfollowerTask(__attribute__((unused)) void *parameters)
courseIntegral = 0;
speedIntegral = 0;
powerIntegral = 0;
-
- break;
}
PathStatusSet(&pathStatus);
}
diff --git a/flight/modules/PathPlanner/pathplanner.c b/flight/modules/PathPlanner/pathplanner.c
index eba9e2092..364ee2480 100644
--- a/flight/modules/PathPlanner/pathplanner.c
+++ b/flight/modules/PathPlanner/pathplanner.c
@@ -138,7 +138,7 @@ static void pathPlannerTask()
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
- if (flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) {
+ if (flightStatus.ControlChain.PathPlanner != FLIGHTSTATUS_CONTROLCHAIN_TRUE) {
pathplanner_active = false;
if (!validPathPlan) {
// unverified path plans are only a warning while we are not in pathplanner mode
diff --git a/flight/modules/Stabilization/stabilization.c b/flight/modules/Stabilization/stabilization.c
index 354bf9eef..db56e9de8 100644
--- a/flight/modules/Stabilization/stabilization.c
+++ b/flight/modules/Stabilization/stabilization.c
@@ -50,7 +50,6 @@
#include "flightstatus.h"
#include "manualcontrolsettings.h"
#include "flightmodesettings.h"
-#include "manualcontrol.h" // Just to get a macro
#include "taskinfo.h"
// Math libraries
@@ -669,7 +668,7 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
}
}
- if (PARSE_FLIGHT_MODE(flightStatus.FlightMode) != FLIGHTMODE_MANUAL) {
+ if (flightStatus.ControlChain.Stabilization == FLIGHTSTATUS_CONTROLCHAIN_TRUE) {
ActuatorDesiredSet(&actuatorDesired);
} else {
// Force all axes to reinitialize when engaged
diff --git a/flight/modules/VtolPathFollower/vtolpathfollower.c b/flight/modules/VtolPathFollower/vtolpathfollower.c
index 101dbec2d..9efcffe3e 100644
--- a/flight/modules/VtolPathFollower/vtolpathfollower.c
+++ b/flight/modules/VtolPathFollower/vtolpathfollower.c
@@ -206,55 +206,53 @@ static void vtolPathFollowerTask(__attribute__((unused)) void *parameters)
PathDesiredGet(&pathDesired);
// Check the combinations of flightmode and pathdesired mode
- switch (flightStatus.FlightMode) {
- case FLIGHTSTATUS_FLIGHTMODE_LAND:
- case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
- case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE:
- if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT) {
- updateEndpointVelocity();
- updateVtolDesiredAttitude(false);
- AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
+ if (flightStatus.ControlChain.PathFollower == FLIGHTSTATUS_CONTROLCHAIN_TRUE) {
+ if (flightStatus.ControlChain.PathPlanner == FLIGHTSTATUS_CONTROLCHAIN_FALSE) {
+ if (flightStatus.FlightMode == FLIGHTSTATUS_FLIGHTMODE_POI) {
+ if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT) {
+ updateEndpointVelocity();
+ updateVtolDesiredAttitude(true);
+ updatePOIBearing();
+ } else {
+ AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR);
+ }
+ } else {
+ if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT) {
+ updateEndpointVelocity();
+ updateVtolDesiredAttitude(false);
+ AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
+ } else {
+ AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR);
+ }
+ }
} else {
- AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR);
+ pathStatus.UID = pathDesired.UID;
+ pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS;
+ switch (pathDesired.Mode) {
+ // TODO: Make updateVtolDesiredAttitude and velocity report success and update PATHSTATUS_STATUS accordingly
+ case PATHDESIRED_MODE_FLYENDPOINT:
+ case PATHDESIRED_MODE_FLYVECTOR:
+ case PATHDESIRED_MODE_FLYCIRCLERIGHT:
+ case PATHDESIRED_MODE_FLYCIRCLELEFT:
+ updatePathVelocity();
+ updateVtolDesiredAttitude(false);
+ AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
+ break;
+ case PATHDESIRED_MODE_FIXEDATTITUDE:
+ updateFixedAttitude(pathDesired.ModeParameters);
+ AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
+ break;
+ case PATHDESIRED_MODE_DISARMALARM:
+ AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL);
+ break;
+ default:
+ pathStatus.Status = PATHSTATUS_STATUS_CRITICAL;
+ AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR);
+ break;
+ }
+ PathStatusSet(&pathStatus);
}
- break;
- case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER:
- pathStatus.UID = pathDesired.UID;
- pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS;
- switch (pathDesired.Mode) {
- // TODO: Make updateVtolDesiredAttitude and velocity report success and update PATHSTATUS_STATUS accordingly
- case PATHDESIRED_MODE_FLYENDPOINT:
- case PATHDESIRED_MODE_FLYVECTOR:
- case PATHDESIRED_MODE_FLYCIRCLERIGHT:
- case PATHDESIRED_MODE_FLYCIRCLELEFT:
- updatePathVelocity();
- updateVtolDesiredAttitude(false);
- AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
- break;
- case PATHDESIRED_MODE_FIXEDATTITUDE:
- updateFixedAttitude(pathDesired.ModeParameters);
- AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
- break;
- case PATHDESIRED_MODE_DISARMALARM:
- AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL);
- break;
- default:
- pathStatus.Status = PATHSTATUS_STATUS_CRITICAL;
- AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR);
- break;
- }
- PathStatusSet(&pathStatus);
- break;
- case FLIGHTSTATUS_FLIGHTMODE_POI:
- if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT) {
- updateEndpointVelocity();
- updateVtolDesiredAttitude(true);
- updatePOIBearing();
- } else {
- AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR);
- }
- break;
- default:
+ } else {
// Be cleaner and get rid of global variables
northVelIntegral = 0;
eastVelIntegral = 0;
@@ -267,8 +265,6 @@ static void vtolPathFollowerTask(__attribute__((unused)) void *parameters)
StabilizationDesiredData stabDesired;
StabilizationDesiredGet(&stabDesired);
thrustOffset = stabDesired.Thrust;
-
- break;
}
AlarmsClear(SYSTEMALARMS_ALARM_GUIDANCE);
diff --git a/shared/uavobjectdefinition/flightstatus.xml b/shared/uavobjectdefinition/flightstatus.xml
index 9c9336c1a..a7009f4c4 100644
--- a/shared/uavobjectdefinition/flightstatus.xml
+++ b/shared/uavobjectdefinition/flightstatus.xml
@@ -6,6 +6,14 @@
+
+
+ Stabilization
+ PathFollower
+ PathPlanner
+
+
+