From 07fafc491347e858ef90c5602e33a23b3bb5ede3 Mon Sep 17 00:00:00 2001 From: tumbili Date: Fri, 15 Jan 2016 12:44:00 +0100 Subject: [PATCH] multirotor mission feasibility checks: make sure that the relative altitude of the takeoff waypoint is at least one meter higher than the acceptance radius of the waypoint. This makes sure that the takeoff waypoint is not reached before the vehicle is at least one meter in the air. --- src/modules/navigator/mission.cpp | 4 +- .../navigator/mission_feasibility_checker.cpp | 41 +++++++++++++++++-- .../navigator/mission_feasibility_checker.h | 4 +- 3 files changed, 41 insertions(+), 8 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 992a426bfe..242b0faedc 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -259,7 +259,7 @@ Mission::update_offboard_mission() dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(), _navigator->get_home_position()->alt, _navigator->home_position_valid(), _navigator->get_global_position()->lat, _navigator->get_global_position()->lon, - _param_dist_1wp.get(), _navigator->get_mission_result()->warning); + _param_dist_1wp.get(), _navigator->get_mission_result()->warning, _navigator->get_acceptance_radius()); _navigator->get_mission_result()->valid = !failed; _navigator->increment_mission_instance_count(); @@ -843,7 +843,7 @@ Mission::check_mission_valid() dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(), _navigator->get_home_position()->alt, _navigator->home_position_valid(), _navigator->get_global_position()->lat, _navigator->get_global_position()->lon, - _param_dist_1wp.get(), _navigator->get_mission_result()->warning); + _param_dist_1wp.get(), _navigator->get_mission_result()->warning, _navigator->get_acceptance_radius()); _navigator->increment_mission_instance_count(); _navigator->set_mission_result_updated(); diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index fd9a206c86..2cf1c45f56 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -64,7 +64,8 @@ MissionFeasibilityChecker::MissionFeasibilityChecker() : bool MissionFeasibilityChecker::checkMissionFeasible(int mavlink_fd, bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, - float home_alt, bool home_valid, double curr_lat, double curr_lon, float max_waypoint_distance, bool &warning_issued) + float home_alt, bool home_valid, double curr_lat, double curr_lon, float max_waypoint_distance, bool &warning_issued, + float default_acceptance_rad) { bool failed = false; bool warned = false; @@ -88,7 +89,7 @@ bool MissionFeasibilityChecker::checkMissionFeasible(int mavlink_fd, bool isRota failed = failed || !checkHomePositionAltitude(dm_current, nMissionItems, home_alt, home_valid, warned); if (isRotarywing) { - failed = failed || !checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence, home_alt, home_valid); + failed = failed || !checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence, home_alt, home_valid, default_acceptance_rad); } else { failed = failed || !checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence, home_alt, home_valid); } @@ -96,9 +97,41 @@ bool MissionFeasibilityChecker::checkMissionFeasible(int mavlink_fd, bool isRota return !failed; } -bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid) +bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, + Geofence &geofence, float home_alt, bool home_valid, float default_acceptance_rad) { - /* no custom rotary wing checks yet */ + /* 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++) { + 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; + } + + // look for a takeoff waypoint + if (missionitem.nav_cmd == NAV_CMD_TAKEOFF) { + // make sure that the altitude of the waypoint is at least one meter larger than the acceptance radius + // this makes sure that the takeoff waypoint is not reached before we are at least one meter in the air + float takeoff_alt = missionitem.altitude_is_relative + ? missionitem.altitude + : missionitem.altitude - home_alt; + // check if we should use default acceptance radius + float acceptance_radius = default_acceptance_rad; + + if (missionitem.acceptance_radius > NAV_EPSILON_POSITION) { + acceptance_radius = missionitem.acceptance_radius; + } + + if (takeoff_alt - 1.0f < acceptance_radius) { + mavlink_log_critical(_mavlink_fd, "Mission rejected: Takeoff altitude too low!"); + return false; + } + } + } + + // all checks have passed return true; } diff --git a/src/modules/navigator/mission_feasibility_checker.h b/src/modules/navigator/mission_feasibility_checker.h index 4586f75a47..a1831999ac 100644 --- a/src/modules/navigator/mission_feasibility_checker.h +++ b/src/modules/navigator/mission_feasibility_checker.h @@ -72,7 +72,7 @@ private: void updateNavigationCapabilities(); /* Checks specific to rotarywing airframes */ - bool checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid); + bool checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid, float default_acceptance_rad); public: MissionFeasibilityChecker(); @@ -83,7 +83,7 @@ public: */ bool checkMissionFeasible(int mavlink_fd, bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid, - double curr_lat, double curr_lon, float max_waypoint_distance, bool &warning_issued); + double curr_lat, double curr_lon, float max_waypoint_distance, bool &warning_issued, float default_acceptance_rad); };