diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2014-01-01 21:54:33 +0100 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2014-01-01 21:54:33 +0100 |
commit | 4191ae33c264459f0a85d9c03b8cb4893c6ee33e (patch) | |
tree | cc6a53355ec205518df963ebe9e897166402d68d /src/modules | |
parent | 09f29d0972dc381817c65eceb0a7072a3e40d59c (diff) | |
download | px4-firmware-4191ae33c264459f0a85d9c03b8cb4893c6ee33e.tar.gz px4-firmware-4191ae33c264459f0a85d9c03b8cb4893c6ee33e.tar.bz2 px4-firmware-4191ae33c264459f0a85d9c03b8cb4893c6ee33e.zip |
navigator/mission feasibility: prepare for pre-mission fence checking
Diffstat (limited to 'src/modules')
-rw-r--r-- | src/modules/navigator/mission_feasibility_checker.cpp | 21 | ||||
-rw-r--r-- | src/modules/navigator/mission_feasibility_checker.h | 10 | ||||
-rw-r--r-- | src/modules/navigator/navigator_main.cpp | 2 |
3 files changed, 17 insertions, 16 deletions
diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index 25b2636bb..cc079dee1 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -48,6 +48,7 @@ #include <stdio.h> #include <fcntl.h> #include <errno.h> +#include <uORB/topics/fence.h> /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -61,7 +62,7 @@ MissionFeasibilityChecker::MissionFeasibilityChecker() : _mavlink_fd(-1), _capab } -bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nItems) +bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, const struct fence_s &fence) { /* Init if not done yet */ init(); @@ -74,39 +75,39 @@ bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_ if (isRotarywing) - return checkMissionFeasibleRotarywing(dm_current, nItems); + return checkMissionFeasibleRotarywing(dm_current, nMissionItems, fence); else - return checkMissionFeasibleFixedwing(dm_current, nItems); + return checkMissionFeasibleFixedwing(dm_current, nMissionItems, fence); } -bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nItems) +bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, const struct fence_s &fence) { - return checkGeofence(dm_current, nItems); + return checkGeofence(dm_current, nMissionItems, fence); } -bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nItems) +bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, const struct fence_s &fence) { /* Update fixed wing navigation capabilites */ updateNavigationCapabilities(); // warnx("_nav_caps.landing_slope_angle_rad %.4f, _nav_caps.landing_horizontal_slope_displacement %.4f", _nav_caps.landing_slope_angle_rad, _nav_caps.landing_horizontal_slope_displacement); - return (checkFixedWingLanding(dm_current, nItems) && checkGeofence(dm_current, nItems)); + return (checkFixedWingLanding(dm_current, nMissionItems) && checkGeofence(dm_current, nMissionItems, fence)); } -bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nItems) +bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMissionItems, const struct fence_s &fence) { //xxx: check geofence return true; } -bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size_t nItems) +bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size_t nMissionItems) { /* Go through all mission items and search for a landing waypoint * if landing waypoint is found: the previous waypoint is checked to be at a feasible distance and altitude given the landing slope */ - for (size_t i = 0; i < nItems; i++) { + for (size_t i = 0; i < nMissionItems; i++) { static struct mission_item_s missionitem; const ssize_t len = sizeof(struct mission_item_s); if (dm_read(dm_current, i, &missionitem, len) != len) { diff --git a/src/modules/navigator/mission_feasibility_checker.h b/src/modules/navigator/mission_feasibility_checker.h index 7d1cc2f8a..ef235ead4 100644 --- a/src/modules/navigator/mission_feasibility_checker.h +++ b/src/modules/navigator/mission_feasibility_checker.h @@ -57,15 +57,15 @@ private: void init(); /* Checks for all airframes */ - bool checkGeofence(dm_item_t dm_current, size_t nItems); + bool checkGeofence(dm_item_t dm_current, size_t nMissionItems, const struct fence_s &fence); /* Checks specific to fixedwing airframes */ - bool checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nItems); - bool checkFixedWingLanding(dm_item_t dm_current, size_t nItems); + bool checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, const struct fence_s &fence); + bool checkFixedWingLanding(dm_item_t dm_current, size_t nMissionItems); void updateNavigationCapabilities(); /* Checks specific to rotarywing airframes */ - bool checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nItems); + bool checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, const struct fence_s &fence); public: MissionFeasibilityChecker(); @@ -74,7 +74,7 @@ public: /* * Returns true if mission is feasible and false otherwise */ - bool checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nItems); + bool checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, const struct fence_s &fence); }; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 982ebefcc..c88f237ad 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -456,7 +456,7 @@ Navigator::offboard_mission_update(bool isrotaryWing) } else { dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; } - missionFeasiblityChecker.checkMissionFeasible(isrotaryWing, dm_current, (size_t)offboard_mission.count); + missionFeasiblityChecker.checkMissionFeasible(isrotaryWing, dm_current, (size_t)offboard_mission.count, _fence); _mission.set_offboard_dataman_id(offboard_mission.dataman_id); _mission.set_current_offboard_mission_index(offboard_mission.current_index); |