From e2a359143d8a07363888a5787b87fd41f8ea55ba Mon Sep 17 00:00:00 2001 From: Lasse Date: Thu, 8 Jun 2017 11:52:46 +0300 Subject: [PATCH] use altitude acceptance radius for MC takeoff check When checking a mission, the takeoff altitude is being checked against the waypoint acceptance radius to ensure the MAV being clear from ground before heading to the next waypoint. However, until now the _horizontal_ acceptance radius was being used, instead of the altitude reference value. Targets PX4/Firmware/#7379 --- src/modules/navigator/mission_feasibility_checker.cpp | 10 ++++++---- src/modules/navigator/mission_feasibility_checker.h | 2 +- 2 files changed, 7 insertions(+), 5 deletions(-) diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index f0ca35eb9c..c947474c13 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -67,6 +67,7 @@ MissionFeasibilityChecker::checkMissionFeasible(const mission_s &mission, bool &warning_issued = _navigator->get_mission_result()->warning; const float default_acceptance_rad = _navigator->get_default_acceptance_radius(); + const float default_altitude_acceptance_rad = _navigator->get_altitude_acceptance_radius(); const bool landed = _navigator->get_land_detected()->landed; bool failed = false; @@ -92,7 +93,7 @@ MissionFeasibilityChecker::checkMissionFeasible(const mission_s &mission, if (isRotarywing) { failed = failed - || !checkRotarywing(dm_current, nMissionItems, home_alt, home_valid, default_acceptance_rad); + || !checkRotarywing(dm_current, nMissionItems, home_alt, home_valid, default_altitude_acceptance_rad); } else { failed = failed @@ -105,7 +106,7 @@ MissionFeasibilityChecker::checkMissionFeasible(const mission_s &mission, bool MissionFeasibilityChecker::checkRotarywing(dm_item_t dm_current, size_t nMissionItems, - float home_alt, bool home_valid, float default_acceptance_rad) + float home_alt, bool home_valid, float default_altitude_acceptance_rad) { for (size_t i = 0; i < nMissionItems; i++) { struct mission_item_s missionitem = {}; @@ -118,13 +119,14 @@ MissionFeasibilityChecker::checkRotarywing(dm_item_t dm_current, size_t nMission // 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 + // make sure that the altitude of the waypoint is at least one meter larger than the altitude 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; + float acceptance_radius = default_altitude_acceptance_rad; + // if a specific acceptance radius has been defined, use that one instead if (missionitem.acceptance_radius > NAV_EPSILON_POSITION) { acceptance_radius = missionitem.acceptance_radius; } diff --git a/src/modules/navigator/mission_feasibility_checker.h b/src/modules/navigator/mission_feasibility_checker.h index 880fd0178c..7d92129b50 100644 --- a/src/modules/navigator/mission_feasibility_checker.h +++ b/src/modules/navigator/mission_feasibility_checker.h @@ -77,7 +77,7 @@ private: /* Checks specific to rotarywing airframes */ bool checkRotarywing(dm_item_t dm_current, size_t nMissionItems, - float home_alt, bool home_valid, float default_acceptance_rad); + float home_alt, bool home_valid, float default_altitude_acceptance_rad); public: MissionFeasibilityChecker(Navigator *navigator) : _navigator(navigator) {}