diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2013-12-25 17:10:38 +0100 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2013-12-25 17:10:38 +0100 |
commit | b02b48290fdb5464020ea49209144ab8d5d045af (patch) | |
tree | db6f8ec161a1d776fbded9c8217e75cdc042c62e /src/modules/navigator | |
parent | d07cc95339e43d4ed3cdf480acf528fb57f989aa (diff) | |
download | px4-firmware-b02b48290fdb5464020ea49209144ab8d5d045af.tar.gz px4-firmware-b02b48290fdb5464020ea49209144ab8d5d045af.tar.bz2 px4-firmware-b02b48290fdb5464020ea49209144ab8d5d045af.zip |
Navigator: add MissionFeasibilityChecker class; performs validation of landing waypoint set-up for fixed wing for now, but can be extended for other checks (e.g. check mission against geofence)
Diffstat (limited to 'src/modules/navigator')
-rw-r--r-- | src/modules/navigator/mission_feasibility_checker.cpp | 184 | ||||
-rw-r--r-- | src/modules/navigator/mission_feasibility_checker.h | 82 | ||||
-rw-r--r-- | src/modules/navigator/module.mk | 3 | ||||
-rw-r--r-- | src/modules/navigator/navigator_main.cpp | 26 |
4 files changed, 288 insertions, 7 deletions
diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp new file mode 100644 index 000000000..25b2636bb --- /dev/null +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -0,0 +1,184 @@ +/**************************************************************************** + * + * 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 + * 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 mission_feasibility_checker.cpp + * Provides checks if mission is feasible given the navigation capabilities + */ + +#include "mission_feasibility_checker.h" + +#include <geo/geo.h> +#include <math.h> +#include <mathlib/mathlib.h> +#include <mavlink/mavlink_log.h> +#include <fw_pos_control_l1/landingslope.h> +#include <systemlib/err.h> +#include <stdio.h> +#include <fcntl.h> +#include <errno.h> + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +MissionFeasibilityChecker::MissionFeasibilityChecker() : _mavlink_fd(-1), _capabilities_sub(-1), _initDone(false) +{ + _nav_caps = {0}; +} + + +bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nItems) +{ + /* Init if not done yet */ + init(); + + /* Open mavlink fd */ + if (_mavlink_fd < 0) { + /* try to open the mavlink log device every once in a while */ + _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + } + + + if (isRotarywing) + return checkMissionFeasibleRotarywing(dm_current, nItems); + else + return checkMissionFeasibleFixedwing(dm_current, nItems); +} + +bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nItems) +{ + + return checkGeofence(dm_current, nItems); +} + +bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nItems) +{ + /* 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)); +} + +bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nItems) +{ + //xxx: check geofence + return true; +} + +bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size_t nItems) +{ + /* 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++) { + 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. */ + return false; + } + + if (missionitem.nav_cmd == NAV_CMD_LAND) { + struct mission_item_s missionitem_previous; + if (i != 0) { + if (dm_read(dm_current, i-1, &missionitem_previous, len) != len) { + /* not supposed to happen unless the datamanager can't access the SD card, etc. */ + return false; + } + + float wp_distance = get_distance_to_next_waypoint(missionitem_previous.lat , missionitem_previous.lon, missionitem.lat, missionitem.lon); + float slope_alt_req = Landingslope::getLandingSlopeAbsoluteAltitude(wp_distance, missionitem.altitude, _nav_caps.landing_horizontal_slope_displacement, _nav_caps.landing_slope_angle_rad); + float wp_distance_req = Landingslope::getLandingSlopeWPDistance(missionitem_previous.altitude, missionitem.altitude, _nav_caps.landing_horizontal_slope_displacement, _nav_caps.landing_slope_angle_rad); + float delta_altitude = missionitem.altitude - missionitem_previous.altitude; +// warnx("wp_distance %.2f, delta_altitude %.2f, missionitem_previous.altitude %.2f, missionitem.altitude %.2f, slope_alt_req %.2f, wp_distance_req %.2f", +// wp_distance, delta_altitude, missionitem_previous.altitude, missionitem.altitude, slope_alt_req, wp_distance_req); +// warnx("_nav_caps.landing_horizontal_slope_displacement %.4f, _nav_caps.landing_slope_angle_rad %.4f, _nav_caps.landing_flare_length %.4f", +// _nav_caps.landing_horizontal_slope_displacement, _nav_caps.landing_slope_angle_rad, _nav_caps.landing_flare_length); + + if (wp_distance > _nav_caps.landing_flare_length) { + /* Last wp is before flare region */ + + if (delta_altitude < 0) { + if (missionitem_previous.altitude <= slope_alt_req) { + /* Landing waypoint is at or below altitude of slope at the given waypoint distance: this is ok, aircraft will intersect the slope */ + return true; + } else { + /* Landing waypoint is above altitude of slope at the given waypoint distance */ + mavlink_log_info(_mavlink_fd, "#audio: Landing: last waypoint too high/too close"); + mavlink_log_info(_mavlink_fd, "Move down to %.1fm or move further away by %.1fm", + (double)(slope_alt_req), + (double)(wp_distance_req - wp_distance)); + return false; + } + } else { + /* Landing waypoint is above last waypoint */ + mavlink_log_info(_mavlink_fd, "#audio: Landing waypoint above last nav waypoint"); + return false; + } + } else { + /* Last wp is in flare region */ + //xxx give recommendations + mavlink_log_info(_mavlink_fd, "#audio: Warning: Landing: last waypoint in flare region"); + return false; + } + } else { + mavlink_log_info(_mavlink_fd, "#audio: Warning: starting with land waypoint"); + return false; + } + } + } + + +// float slope_alt = wp_altitude + _H0 * expf(-math::max(0.0f, _flare_length - wp_distance)/_flare_constant) - _H1_virt; +} + +void MissionFeasibilityChecker::updateNavigationCapabilities() +{ + int res = orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps); +} + +void MissionFeasibilityChecker::init() +{ + if (!_initDone) { + + _capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities)); + + _initDone = true; + } +} diff --git a/src/modules/navigator/mission_feasibility_checker.h b/src/modules/navigator/mission_feasibility_checker.h new file mode 100644 index 000000000..7d1cc2f8a --- /dev/null +++ b/src/modules/navigator/mission_feasibility_checker.h @@ -0,0 +1,82 @@ +/**************************************************************************** + * + * 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 + * 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 mission_feasibility_checker.h + * Provides checks if mission is feasible given the navigation capabilities + */ +#ifndef MISSION_FEASIBILITY_CHECKER_H_ +#define MISSION_FEASIBILITY_CHECKER_H_ + +#include <unistd.h> +#include <uORB/topics/mission.h> +#include <uORB/topics/navigation_capabilities.h> +#include <dataman/dataman.h> + + +class MissionFeasibilityChecker +{ +private: + int _mavlink_fd; + + int _capabilities_sub; + struct navigation_capabilities_s _nav_caps; + + bool _initDone; + void init(); + + /* Checks for all airframes */ + bool checkGeofence(dm_item_t dm_current, size_t nItems); + + /* 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); + void updateNavigationCapabilities(); + + /* Checks specific to rotarywing airframes */ + bool checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nItems); +public: + + MissionFeasibilityChecker(); + ~MissionFeasibilityChecker() {} + + /* + * Returns true if mission is feasible and false otherwise + */ + bool checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nItems); + +}; + + +#endif /* MISSION_FEASIBILITY_CHECKER_H_ */ diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk index fc59c956a..6be4e87a0 100644 --- a/src/modules/navigator/module.mk +++ b/src/modules/navigator/module.mk @@ -39,6 +39,7 @@ MODULE_COMMAND = navigator SRCS = navigator_main.cpp \ navigator_params.c \ - navigator_mission.cpp + navigator_mission.cpp \ + mission_feasibility_checker.cpp INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 48f828ff7..c7ac885b4 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -76,6 +76,7 @@ #include <mavlink/mavlink_log.h> #include "navigator_mission.h" +#include "mission_feasibility_checker.h" /* oddly, ERROR is not defined for c++ */ @@ -165,6 +166,8 @@ private: bool _waypoint_yaw_reached; uint64_t _time_first_inside_orbit; + MissionFeasibilityChecker missionFeasiblityChecker; + struct { float min_altitude; float loiter_radius; @@ -228,7 +231,7 @@ private: /** * Retrieve offboard mission. */ - void offboard_mission_update(); + void offboard_mission_update(bool isrotaryWing); /** * Retrieve onboard mission. @@ -435,11 +438,21 @@ Navigator::navigation_capabilities_update() void -Navigator::offboard_mission_update() +Navigator::offboard_mission_update(bool isrotaryWing) { struct mission_s offboard_mission; if (orb_copy(ORB_ID(mission), _offboard_mission_sub, &offboard_mission) == OK) { + /* Check mission feasibility, for now do not handle the return value, + * however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */ + dm_item_t dm_current; + if (offboard_mission.dataman_id == 0) { + dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; + } else { + dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; + } + missionFeasiblityChecker.checkMissionFeasible(isrotaryWing, dm_current, (size_t)offboard_mission.count); + _mission.set_offboard_dataman_id(offboard_mission.dataman_id); _mission.set_current_offboard_mission_index(offboard_mission.current_index); _mission.set_offboard_mission_count(offboard_mission.count); @@ -503,13 +516,14 @@ Navigator::task_main() /* copy all topics first time */ + vehicle_status_update(); parameters_update(); global_position_update(); home_position_update(); navigation_capabilities_update(); - offboard_mission_update(); + offboard_mission_update(_vstatus.is_rotary_wing); onboard_mission_update(); - vehicle_status_update(); + /* rate limit vehicle status updates to 5Hz */ orb_set_interval(_vstatus_sub, 200); @@ -632,7 +646,7 @@ Navigator::task_main() } if (fds[4].revents & POLLIN) { - offboard_mission_update(); + offboard_mission_update(_vstatus.is_rotary_wing); // XXX check if mission really changed dispatch(EVENT_MISSION_CHANGED); } @@ -1311,4 +1325,4 @@ Navigator::add_home_pos_to_rtl(struct mission_item_s *new_mission_item) new_mission_item->loiter_radius = _parameters.loiter_radius; // TODO: get rid of magic number new_mission_item->radius = 50.0f; // TODO: get rid of magic number } -}
\ No newline at end of file +} |