diff options
Diffstat (limited to 'src/modules/navigator')
| -rw-r--r-- | src/modules/navigator/geofence.cpp | 5 | ||||
| -rw-r--r-- | src/modules/navigator/geofence.h | 5 | ||||
| -rw-r--r-- | src/modules/navigator/geofence_params.c | 1 | ||||
| -rw-r--r-- | src/modules/navigator/mission_feasibility_checker.cpp | 59 | ||||
| -rw-r--r-- | src/modules/navigator/mission_feasibility_checker.h | 13 | ||||
| -rw-r--r-- | src/modules/navigator/module.mk | 2 | ||||
| -rw-r--r-- | src/modules/navigator/navigator_main.cpp | 14 | ||||
| -rw-r--r-- | src/modules/navigator/navigator_mission.cpp | 3 | ||||
| -rw-r--r-- | src/modules/navigator/navigator_mission.h | 3 | ||||
| -rw-r--r-- | src/modules/navigator/navigator_params.c | 3 | ||||
| -rw-r--r-- | src/modules/navigator/navigator_state.h | 42 |
11 files changed, 108 insertions, 42 deletions
diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index f452a85f7..bc8dbca50 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -1,8 +1,6 @@ /**************************************************************************** * * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: @author Jean Cyr <jean.m.cyr@gmail.com> - * @author Thomas Gubler <thomasgubler@gmail.com> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,6 +33,9 @@ /** * @file geofence.cpp * Provides functions for handling the geofence + * + * @author Jean Cyr <jean.m.cyr@gmail.com> + * @author Thomas Gubler <thomasgubler@gmail.com> */ #include "geofence.h" diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h index 9628b7271..2eb126ab5 100644 --- a/src/modules/navigator/geofence.h +++ b/src/modules/navigator/geofence.h @@ -1,8 +1,6 @@ /**************************************************************************** * * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: @author Jean Cyr <jean.m.cyr@gmail.com> - * @author Thomas Gubler <thomasgubler@gmail.com> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,6 +33,9 @@ /** * @file geofence.h * Provides functions for handling the geofence + * + * @author Jean Cyr <jean.m.cyr@gmail.com> + * @author Thomas Gubler <thomasgubler@gmail.com> */ #ifndef GEOFENCE_H_ diff --git a/src/modules/navigator/geofence_params.c b/src/modules/navigator/geofence_params.c index 5831a0ca9..653b1ad84 100644 --- a/src/modules/navigator/geofence_params.c +++ b/src/modules/navigator/geofence_params.c @@ -1,7 +1,6 @@ /**************************************************************************** * * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier <lm@inf.ethz.ch> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index eaafa217d..e1a6854b2 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -1,8 +1,6 @@ /**************************************************************************** * * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier <lm@inf.ethz.ch> - * @author Thomas Gubler <thomasgubler@student.ethz.ch> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,6 +33,9 @@ /** * @file mission_feasibility_checker.cpp * Provides checks if mission is feasible given the navigation capabilities + * + * @author Lorenz Meier <lm@inf.ethz.ch> + * @author Thomas Gubler <thomasgubler@student.ethz.ch> */ #include "mission_feasibility_checker.h" @@ -62,7 +63,7 @@ MissionFeasibilityChecker::MissionFeasibilityChecker() : _mavlink_fd(-1), _capab } -bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence) +bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt) { /* Init if not done yet */ init(); @@ -75,24 +76,24 @@ bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_ if (isRotarywing) - return checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence); + return checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence, home_alt); else - return checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence); + return checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence, home_alt); } -bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence) +bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt) { - return checkGeofence(dm_current, nMissionItems, geofence); + return (checkGeofence(dm_current, nMissionItems, geofence) && checkHomePositionAltitude(dm_current, nMissionItems, home_alt)); } -bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence) +bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt) { /* 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, geofence)); + return (checkFixedWingLanding(dm_current, nMissionItems) && checkGeofence(dm_current, nMissionItems, geofence) && checkHomePositionAltitude(dm_current, nMissionItems, home_alt)); } bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence) @@ -100,15 +101,15 @@ bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMiss /* Check if all mission items are inside the geofence (if we have a valid geofence) */ if (geofence.valid()) { for (size_t i = 0; i < nMissionItems; i++) { - static struct mission_item_s missionitem; - const ssize_t len = sizeof(struct mission_item_s); + struct mission_item_s missionitem; + const ssize_t len = sizeof(missionitem); if (dm_read(dm_current, i, &missionitem, len) != len) { /* not supposed to happen unless the datamanager can't access the SD card, etc. */ return false; } - if (!geofence.inside(missionitem.lat, missionitem.lon, missionitem.altitude)) { //xxx: handle relative altitude + if (!geofence.inside(missionitem.lat, missionitem.lon, missionitem.altitude)) { mavlink_log_info(_mavlink_fd, "#audio: Geofence violation waypoint %d", i); return false; } @@ -118,6 +119,36 @@ bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMiss return true; } +bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool throw_error) +{ + /* Check if all all waypoints are above the home altitude, only return false if bool throw_error = true */ + 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) { + /* not supposed to happen unless the datamanager can't access the SD card, etc. */ + if (throw_error) { + return false; + } else { + return true; + } + } + + if (home_alt > missionitem.altitude) { + if (throw_error) { + mavlink_log_info(_mavlink_fd, "Waypoint %d below home", i); + return false; + } else { + mavlink_log_info(_mavlink_fd, "#audio: warning waypoint %d below home", i); + return true; + } + } + } + + return true; +} + bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size_t nMissionItems) { /* Go through all mission items and search for a landing waypoint @@ -125,8 +156,8 @@ bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size for (size_t i = 0; i < nMissionItems; i++) { - static struct mission_item_s missionitem; - const ssize_t len = sizeof(struct mission_item_s); + struct mission_item_s missionitem; + const ssize_t len = sizeof(missionitem); if (dm_read(dm_current, i, &missionitem, len) != len) { /* not supposed to happen unless the datamanager can't access the SD card, etc. */ return false; diff --git a/src/modules/navigator/mission_feasibility_checker.h b/src/modules/navigator/mission_feasibility_checker.h index 7a0b2a296..96c9209d3 100644 --- a/src/modules/navigator/mission_feasibility_checker.h +++ b/src/modules/navigator/mission_feasibility_checker.h @@ -1,8 +1,6 @@ /**************************************************************************** * * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier <lm@inf.ethz.ch> - * @author Thomas Gubler <thomasgubler@student.ethz.ch> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,7 +33,11 @@ /** * @file mission_feasibility_checker.h * Provides checks if mission is feasible given the navigation capabilities + * + * @author Lorenz Meier <lm@inf.ethz.ch> + * @author Thomas Gubler <thomasgubler@student.ethz.ch> */ + #ifndef MISSION_FEASIBILITY_CHECKER_H_ #define MISSION_FEASIBILITY_CHECKER_H_ @@ -59,14 +61,15 @@ private: /* Checks for all airframes */ bool checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence); + bool checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool throw_error = false); /* Checks specific to fixedwing airframes */ - bool checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence); + bool checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt); 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, Geofence &geofence); + bool checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt); public: MissionFeasibilityChecker(); @@ -75,7 +78,7 @@ public: /* * Returns true if mission is feasible and false otherwise */ - bool checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence); + bool checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt); }; diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk index 55f8a4caa..6ea9dec2b 100644 --- a/src/modules/navigator/module.mk +++ b/src/modules/navigator/module.mk @@ -45,3 +45,5 @@ SRCS = navigator_main.cpp \ geofence_params.c INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink + +MODULE_STACKSIZE = 1200 diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 12fd35a0a..401d50f7e 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1,10 +1,6 @@ /**************************************************************************** * * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier <lm@inf.ethz.ch> - * @author Jean Cyr <jean.m.cyr@gmail.com> - * @author Julian Oes <joes@student.ethz.ch> - * @author Anton Babushkin <anton.babushkin@me.com> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,7 +31,7 @@ * ****************************************************************************/ /** - * @file navigator_main.c + * @file navigator_main.cpp * Implementation of the main navigation state machine. * * Handles missions, geo fencing and failsafe navigation behavior. @@ -523,7 +519,7 @@ Navigator::offboard_mission_update(bool isrotaryWing) dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; } - missionFeasiblityChecker.checkMissionFeasible(isrotaryWing, dm_current, (size_t)offboard_mission.count, _geofence); + missionFeasiblityChecker.checkMissionFeasible(isrotaryWing, dm_current, (size_t)offboard_mission.count, _geofence, _home_pos.alt); _mission.set_offboard_dataman_id(offboard_mission.dataman_id); @@ -852,7 +848,7 @@ Navigator::start() _navigator_task = task_spawn_cmd("navigator", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, - 2048, + 2000, (main_t)&Navigator::task_main_trampoline, nullptr); @@ -1291,7 +1287,7 @@ Navigator::set_rtl_item() _mission_item.yaw = NAN; _mission_item.loiter_radius = _parameters.loiter_radius; _mission_item.loiter_direction = 1; - _mission_item.nav_cmd = NAV_CMD_TAKEOFF; + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; _mission_item.acceptance_radius = _parameters.acceptance_radius; _mission_item.time_inside = 0.0f; _mission_item.pitch_min = 0.0f; @@ -1351,7 +1347,7 @@ Navigator::set_rtl_item() _mission_item.yaw = NAN; _mission_item.loiter_radius = _parameters.loiter_radius; _mission_item.loiter_direction = 1; - _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; _mission_item.acceptance_radius = _parameters.acceptance_radius; _mission_item.time_inside = _parameters.rtl_land_delay < 0.0f ? 0.0f : _parameters.rtl_land_delay; _mission_item.pitch_min = 0.0f; diff --git a/src/modules/navigator/navigator_mission.cpp b/src/modules/navigator/navigator_mission.cpp index 72dddebfe..49fc62785 100644 --- a/src/modules/navigator/navigator_mission.cpp +++ b/src/modules/navigator/navigator_mission.cpp @@ -1,7 +1,6 @@ /**************************************************************************** * * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: @author Julian Oes <joes@student.ethz.ch> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -34,6 +33,8 @@ /** * @file navigator_mission.cpp * Helper class to access missions + * + * @author Julian Oes <joes@student.ethz.ch> */ #include <string.h> diff --git a/src/modules/navigator/navigator_mission.h b/src/modules/navigator/navigator_mission.h index 2bd4da82e..b0f88e016 100644 --- a/src/modules/navigator/navigator_mission.h +++ b/src/modules/navigator/navigator_mission.h @@ -1,7 +1,6 @@ /**************************************************************************** * * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: @author Julian Oes <joes@student.ethz.ch> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -34,6 +33,8 @@ /** * @file navigator_mission.h * Helper class to access missions + * + * @author Julian Oes <joes@student.ethz.ch> */ #ifndef NAVIGATOR_MISSION_H diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c index 9ef359c6d..5139283b6 100644 --- a/src/modules/navigator/navigator_params.c +++ b/src/modules/navigator/navigator_params.c @@ -1,9 +1,6 @@ /**************************************************************************** * * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier <lm@inf.ethz.ch> - * @author Julian Oes <joes@student.ethz.ch> - * @author Anton Babushkin <anton.babushkin@me.com> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/src/modules/navigator/navigator_state.h b/src/modules/navigator/navigator_state.h index 6a1475c9b..476f93414 100644 --- a/src/modules/navigator/navigator_state.h +++ b/src/modules/navigator/navigator_state.h @@ -1,8 +1,42 @@ -/* - * navigator_state.h +/**************************************************************************** * - * Created on: 27.01.2014 - * Author: ton + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file navigator_state.h + * + * Navigator state + * + * @author Anton Babushkin <anton.babushkin@me.com> */ #ifndef NAVIGATOR_STATE_H_ |