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 + + +