From 4191ae33c264459f0a85d9c03b8cb4893c6ee33e Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 1 Jan 2014 21:54:33 +0100 Subject: navigator/mission feasibility: prepare for pre-mission fence checking --- .../navigator/mission_feasibility_checker.cpp | 21 +++++++++++---------- 1 file changed, 11 insertions(+), 10 deletions(-) (limited to 'src/modules/navigator/mission_feasibility_checker.cpp') 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 #include #include +#include /* 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) { -- cgit v1.2.3 From 429a11a21d25e34ca711b2c0debb2ac3e84c45ca Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 2 Jan 2014 15:01:08 +0100 Subject: navigator/geofence: move more functions to geofence class (WIP) --- src/modules/navigator/geofence.cpp | 97 +++++++++++++++- src/modules/navigator/geofence.h | 16 +++ .../navigator/mission_feasibility_checker.cpp | 16 +-- .../navigator/mission_feasibility_checker.h | 9 +- src/modules/navigator/navigator_main.cpp | 127 +++------------------ 5 files changed, 138 insertions(+), 127 deletions(-) (limited to 'src/modules/navigator/mission_feasibility_checker.cpp') diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index bc5ef44f2..4a0528b16 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -39,10 +39,14 @@ #include "geofence.h" #include +#include +#include +#include +#include -Geofence::Geofence() +Geofence::Geofence() : _fence_pub(-1) { - + memset(&_fence, 0, sizeof(_fence)); } Geofence::~Geofence() @@ -71,3 +75,92 @@ bool Geofence::inside(const struct vehicle_global_position_s *vehicle) c = !c; return c; } + +bool +Geofence::load(unsigned vertices) +{ + struct fence_s temp_fence; + + unsigned i; + for (i = 0; i < vertices; i++) { + if (dm_read(DM_KEY_FENCE_POINTS, i, temp_fence.vertices + i, sizeof(struct fence_vertex_s)) != sizeof(struct fence_vertex_s)) { + break; + } + } + + temp_fence.count = i; + + if (valid()) + memcpy(&_fence, &temp_fence, sizeof(_fence)); + else + warnx("Invalid fence file, ignored!"); + + return _fence.count != 0; +} + +bool +Geofence::valid() +{ + // NULL fence is valid + if (_fence.count == 0) { + return true; + } + + // Otherwise + if ((_fence.count < 4) || (_fence.count > GEOFENCE_MAX_VERTICES)) { + warnx("Fence must have at least 3 sides and not more than %d", GEOFENCE_MAX_VERTICES - 1); + return false; + } + + return true; +} + +void +Geofence::addPoint(int argc, char *argv[]) +{ + int ix, last; + double lon, lat; + struct fence_vertex_s vertex; + char *end; + + if ((argc == 1) && (strcmp("-clear", argv[0]) == 0)) { + dm_clear(DM_KEY_FENCE_POINTS); + publishFence(0); + return; + } + + if (argc < 3) + errx(1, "Specify: -clear | sequence latitude longitude [-publish]"); + + ix = atoi(argv[0]); + if (ix >= DM_KEY_FENCE_POINTS_MAX) + errx(1, "Sequence must be less than %d", DM_KEY_FENCE_POINTS_MAX); + + lat = strtod(argv[1], &end); + lon = strtod(argv[2], &end); + + last = 0; + if ((argc > 3) && (strcmp(argv[3], "-publish") == 0)) + last = 1; + + vertex.lat = (float)lat; + vertex.lon = (float)lon; + + if (dm_write(DM_KEY_FENCE_POINTS, ix, DM_PERSIST_POWER_ON_RESET, &vertex, sizeof(vertex)) == sizeof(vertex)) { + if (last) + publishFence((unsigned)ix + 1); + return; + } + + errx(1, "can't store fence point"); +} + +void +Geofence::publishFence(unsigned vertices) +{ + if (_fence_pub == -1) + _fence_pub = orb_advertise(ORB_ID(fence), &vertices); + else + orb_publish(ORB_ID(fence), _fence_pub, &vertices); +} + diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h index 6d6e6e87c..8f3a07b02 100644 --- a/src/modules/navigator/geofence.h +++ b/src/modules/navigator/geofence.h @@ -45,6 +45,7 @@ class Geofence { private: struct fence_s _fence; + orb_advert_t _fence_pub; /**< publish fence topic */ public: Geofence(); ~Geofence(); @@ -58,6 +59,21 @@ public: * @return true: craft is inside fence, false:craft is outside fence */ bool inside(const struct vehicle_global_position_s *craft); + + + /** + * Load fence parameters. + */ + bool load(unsigned vertices); + + bool valid(); + + /** + * Specify fence vertex position. + */ + void addPoint(int argc, char *argv[]); + + void publishFence(unsigned vertices); }; diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index cc079dee1..aba2dffff 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -62,7 +62,7 @@ MissionFeasibilityChecker::MissionFeasibilityChecker() : _mavlink_fd(-1), _capab } -bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, const struct fence_s &fence) +bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence) { /* Init if not done yet */ init(); @@ -75,27 +75,27 @@ bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_ if (isRotarywing) - return checkMissionFeasibleRotarywing(dm_current, nMissionItems, fence); + return checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence); else - return checkMissionFeasibleFixedwing(dm_current, nMissionItems, fence); + return checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence); } -bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, const struct fence_s &fence) +bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence) { - return checkGeofence(dm_current, nMissionItems, fence); + return checkGeofence(dm_current, nMissionItems, geofence); } -bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, const struct fence_s &fence) +bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence) { /* 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, nMissionItems) && checkGeofence(dm_current, nMissionItems, fence)); + return (checkFixedWingLanding(dm_current, nMissionItems) && checkGeofence(dm_current, nMissionItems, geofence)); } -bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMissionItems, const struct fence_s &fence) +bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence) { //xxx: check geofence return true; diff --git a/src/modules/navigator/mission_feasibility_checker.h b/src/modules/navigator/mission_feasibility_checker.h index ef235ead4..7a0b2a296 100644 --- a/src/modules/navigator/mission_feasibility_checker.h +++ b/src/modules/navigator/mission_feasibility_checker.h @@ -43,6 +43,7 @@ #include #include #include +#include "geofence.h" class MissionFeasibilityChecker @@ -57,15 +58,15 @@ private: void init(); /* Checks for all airframes */ - bool checkGeofence(dm_item_t dm_current, size_t nMissionItems, const struct fence_s &fence); + bool checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence); /* Checks specific to fixedwing airframes */ - bool checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, const struct fence_s &fence); + bool checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence); 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 nMissionItems, const struct fence_s &fence); + bool checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence); public: MissionFeasibilityChecker(); @@ -74,7 +75,7 @@ public: /* * Returns true if mission is feasible and false otherwise */ - bool checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, const struct fence_s &fence); + bool checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence); }; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index a9547355f..354fa733b 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -120,14 +120,9 @@ public: void status(); /** - * Load fence parameters. + * Add point to geofence */ - bool load_fence(unsigned vertices); - - /** - * Specify fence vertex position. - */ - void fence_point(int argc, char *argv[]); + void add_fence_point(int argc, char *argv[]); private: @@ -145,7 +140,6 @@ private: int _capabilities_sub; /**< notification of vehicle capabilities updates */ orb_advert_t _triplet_pub; /**< publish position setpoint triplet */ - orb_advert_t _fence_pub; /**< publish fence topic */ orb_advert_t _mission_result_pub; /**< publish mission result topic */ orb_advert_t _control_mode_pub; /**< publish vehicle control mode topic */ @@ -158,8 +152,7 @@ private: perf_counter_t _loop_perf; /**< loop performance counter */ - Geofence geofence; - struct fence_s _fence; /**< storage for fence vertices */ + Geofence _geofence; bool _fence_valid; /**< flag if fence is valid */ bool _inside_fence; /**< vehicle is inside fence */ @@ -252,12 +245,8 @@ private: */ void task_main() __attribute__((noreturn)); - void publish_fence(unsigned vertices); - void publish_safepoints(unsigned points); - bool fence_valid(const struct fence_s &fence); - /** * Functions that are triggered when a new state is entered. */ @@ -347,7 +336,6 @@ Navigator::Navigator() : /* publications */ _triplet_pub(-1), - _fence_pub(-1), _mission_result_pub(-1), _control_mode_pub(-1), @@ -365,8 +353,6 @@ Navigator::Navigator() : _time_first_inside_orbit(0), _set_nav_state_timestamp(0) { - memset(&_fence, 0, sizeof(_fence)); - _parameter_handles.min_altitude = param_find("NAV_MIN_ALT"); _parameter_handles.loiter_radius = param_find("NAV_LOITER_RAD"); _parameter_handles.onboard_mission_enabled = param_find("NAV_ONB_MIS_EN"); @@ -458,7 +444,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, _fence); + missionFeasiblityChecker.checkMissionFeasible(isrotaryWing, dm_current, (size_t)offboard_mission.count, _geofence); _mission.set_offboard_dataman_id(offboard_mission.dataman_id); _mission.set_current_offboard_mission_index(offboard_mission.current_index); @@ -511,7 +497,7 @@ Navigator::task_main() mavlink_log_info(_mavlink_fd, "[navigator] started"); - _fence_valid = load_fence(GEOFENCE_MAX_VERTICES); + _fence_valid = _geofence.load(GEOFENCE_MAX_VERTICES); /* * do subscriptions @@ -782,9 +768,9 @@ Navigator::status() } if (_fence_valid) { warnx("Geofence is valid"); - warnx("Vertex longitude latitude"); - for (unsigned i = 0; i < _fence.count; i++) - warnx("%6u %9.5f %8.5f", i, (double)_fence.vertices[i].lon, (double)_fence.vertices[i].lat); +// warnx("Vertex longitude latitude"); +// for (unsigned i = 0; i < _fence.count; i++) +// warnx("%6u %9.5f %8.5f", i, (double)_fence.vertices[i].lon, (double)_fence.vertices[i].lat); } else { warnx("Geofence not set"); } @@ -808,96 +794,6 @@ Navigator::status() } } -void -Navigator::publish_fence(unsigned vertices) -{ - if (_fence_pub == -1) - _fence_pub = orb_advertise(ORB_ID(fence), &vertices); - else - orb_publish(ORB_ID(fence), _fence_pub, &vertices); -} - -bool -Navigator::fence_valid(const struct fence_s &fence) -{ - // NULL fence is valid - if (fence.count == 0) { - return true; - } - - // Otherwise - if ((fence.count < 4) || (fence.count > GEOFENCE_MAX_VERTICES)) { - warnx("Fence must have at least 3 sides and not more than %d", GEOFENCE_MAX_VERTICES - 1); - return false; - } - - return true; -} - -bool -Navigator::load_fence(unsigned vertices) -{ - struct fence_s temp_fence; - - unsigned i; - for (i = 0; i < vertices; i++) { - if (dm_read(DM_KEY_FENCE_POINTS, i, temp_fence.vertices + i, sizeof(struct fence_vertex_s)) != sizeof(struct fence_vertex_s)) { - break; - } - } - - temp_fence.count = i; - - if (fence_valid(temp_fence)) - memcpy(&_fence, &temp_fence, sizeof(_fence)); - else - warnx("Invalid fence file, ignored!"); - - return _fence.count != 0; -} - -void -Navigator::fence_point(int argc, char *argv[]) -{ - int ix, last; - double lon, lat; - struct fence_vertex_s vertex; - char *end; - - if ((argc == 1) && (strcmp("-clear", argv[0]) == 0)) { - dm_clear(DM_KEY_FENCE_POINTS); - publish_fence(0); - return; - } - - if (argc < 3) - errx(1, "Specify: -clear | sequence latitude longitude [-publish]"); - - ix = atoi(argv[0]); - if (ix >= DM_KEY_FENCE_POINTS_MAX) - errx(1, "Sequence must be less than %d", DM_KEY_FENCE_POINTS_MAX); - - lat = strtod(argv[1], &end); - lon = strtod(argv[2], &end); - - last = 0; - if ((argc > 3) && (strcmp(argv[3], "-publish") == 0)) - last = 1; - - vertex.lat = (float)lat; - vertex.lon = (float)lon; - - if (dm_write(DM_KEY_FENCE_POINTS, ix, DM_PERSIST_POWER_ON_RESET, &vertex, sizeof(vertex)) == sizeof(vertex)) { - if (last) - publish_fence((unsigned)ix + 1); - return; - } - - errx(1, "can't store fence point"); -} - - - StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { { /* STATE_NONE */ @@ -1351,6 +1247,11 @@ bool Navigator::cmp_mission_item_equivalent(const struct mission_item_s a, const } } +void Navigator::add_fence_point(int argc, char *argv[]) +{ + _geofence.addPoint(argc, argv); +} + static void usage() { @@ -1395,7 +1296,7 @@ int navigator_main(int argc, char *argv[]) navigator::g_navigator->status(); } else if (!strcmp(argv[1], "fence")) { - navigator::g_navigator->fence_point(argc - 2, argv + 2); + navigator::g_navigator->add_fence_point(argc - 2, argv + 2); } else { usage(); -- cgit v1.2.3