diff --git a/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c b/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c index 3226d80bd..5858bf5e0 100644 --- a/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c +++ b/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c @@ -45,13 +45,7 @@ #include -#include -#include - #include "hwsettings.h" -#include "flightplan.h" -#include "waypoint.h" -#include "pathaction.h" #include "attitudestate.h" #include "pathdesired.h" // object that will be updated by the module #include "positionstate.h" @@ -68,6 +62,7 @@ #include "velocitydesired.h" #include "velocitystate.h" #include "taskinfo.h" +#include #include "paths.h" #include "CoordinateConversions.h" @@ -85,7 +80,6 @@ static PathStatusData pathStatus; static FixedWingPathFollowerSettingsData fixedwingpathfollowerSettings; // Private functions -static bool checkFlightPlan(); static void pathfollowerTask(void *parameters); static void SettingsUpdatedCb(UAVObjEvent *ev); static void updatePathVelocity(); @@ -169,7 +163,6 @@ static void pathfollowerTask(__attribute__((unused)) void *parameters) // 1. Must have FixedWing type airframe // 2. Flight mode is PositionHold and PathDesired.Mode is Endpoint OR // FlightMode is PathPlanner and PathDesired.Mode is Endpoint or Path - // 3. FlightPlan must be valid SystemSettingsGet(&systemSettings); if ((systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWING) && @@ -179,15 +172,11 @@ static void pathfollowerTask(__attribute__((unused)) void *parameters) vTaskDelay(1000); continue; } - if (!checkFlightPlan()) { - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING); - vTaskDelay(1000); - continue; - } // Continue collecting data if not enough time vTaskDelayUntil(&lastUpdateTime, fixedwingpathfollowerSettings.UpdatePeriod / portTICK_RATE_MS); + FlightStatusGet(&flightStatus); PathStatusGet(&pathStatus); @@ -253,48 +242,6 @@ static void pathfollowerTask(__attribute__((unused)) void *parameters) } } -static bool checkFlightPlan() -{ - uint16_t i; - uint16_t waypointCount; - uint16_t actionCount; - uint8_t flightCrc; - FlightPlanData flightPlan; - - FlightPlanGet(&flightPlan); - - waypointCount = flightPlan.WaypointCount; - actionCount = flightPlan.PathActionCount; - - // check count consistency - if (waypointCount > UAVObjGetNumInstances(WaypointHandle())) { - PIOS_DEBUGLOG_Printf("FlighPlan : waypoint count error!"); - return false; - } - if (actionCount > UAVObjGetNumInstances(PathActionHandle())) { - PIOS_DEBUGLOG_Printf("FlighPlan : path action count error!"); - return false; - } - - // check CRC - flightCrc = 0; - for(i = 0; i < waypointCount; i++) { - flightCrc = UAVObjUpdateCRC(WaypointHandle(), i, flightCrc); - } - for(i = 0; i < actionCount; i++) { - flightCrc = UAVObjUpdateCRC(PathActionHandle(), i, flightCrc); - } - if (flightCrc != flightPlan.Crc) { - PIOS_DEBUGLOG_Printf("FlighPlan : bad CRC (%d / %d)!", flightCrc, flightPlan.Crc); - return false; - } - - // everything ok (hopefully...) - PIOS_DEBUGLOG_Printf("FlighPlan : passed consistency check.", flightCrc, flightPlan.Crc); - - return true; -} - /** * Compute desired velocity from the current position and path *