From bc48634101dff97b7e4cd2d5e3008613a1c0df6d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 11 Jun 2015 12:24:26 +0200 Subject: [PATCH] Navigator: Reject missions with relative altitude if no home was set before arming --- src/modules/navigator/mission.cpp | 2 +- .../navigator/mission_feasibility_checker.cpp | 23 ++++++++++++------- .../navigator/mission_feasibility_checker.h | 8 +++---- src/modules/navigator/navigator.h | 1 + 4 files changed, 21 insertions(+), 13 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 98b01c7578..4eaeca1d8e 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -232,7 +232,7 @@ Mission::update_offboard_mission() failed = !_missionFeasiblityChecker.checkMissionFeasible(_navigator->get_vstatus()->is_rotary_wing, dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(), - _navigator->get_home_position()->alt); + _navigator->get_home_position()->alt, _navigator->home_position_valid()); } else { warnx("offboard mission update failed"); diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index d9297f25bf..19baa79fcd 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -63,7 +63,7 @@ MissionFeasibilityChecker::MissionFeasibilityChecker() : _mavlink_fd(-1), _capab } -bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt) +bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid) { bool failed = false; /* Init if not done yet */ @@ -80,25 +80,25 @@ bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_ if (isRotarywing) - failed |= !checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence, home_alt); + failed |= !checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence, home_alt, home_valid); else - failed |= !checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence, home_alt); + failed |= !checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence, home_alt, home_valid); return !failed; } -bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt) +bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid) { /* Perform checks and issue feedback to the user for all checks */ bool resGeofence = checkGeofence(dm_current, nMissionItems, geofence); - bool resHomeAltitude = checkHomePositionAltitude(dm_current, nMissionItems, home_alt); + bool resHomeAltitude = checkHomePositionAltitude(dm_current, nMissionItems, home_alt, home_valid); /* Mission is only marked as feasible if all checks return true */ return (resGeofence && resHomeAltitude); } -bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt) +bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid) { /* Update fixed wing navigation capabilites */ updateNavigationCapabilities(); @@ -107,7 +107,7 @@ bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_curre /* Perform checks and issue feedback to the user for all checks */ bool resLanding = checkFixedWingLanding(dm_current, nMissionItems); bool resGeofence = checkGeofence(dm_current, nMissionItems, geofence); - bool resHomeAltitude = checkHomePositionAltitude(dm_current, nMissionItems, home_alt); + bool resHomeAltitude = checkHomePositionAltitude(dm_current, nMissionItems, home_alt, home_valid); /* Mission is only marked as feasible if all checks return true */ return (resLanding && resGeofence && resHomeAltitude); @@ -136,7 +136,7 @@ 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) +bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool home_valid, 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++) { @@ -152,6 +152,13 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, } } + if (missionitem.altitude_is_relative && !home_valid) { + if (throw_error) { + mavlink_log_critical(_mavlink_fd, "Rejecting Mission: No home pos, WP %d uses rel alt", i); + return false; + } + } + /* calculate the global waypoint altitude */ float wp_alt = (missionitem.altitude_is_relative) ? missionitem.altitude + home_alt : missionitem.altitude; diff --git a/src/modules/navigator/mission_feasibility_checker.h b/src/modules/navigator/mission_feasibility_checker.h index 94b6b29226..964bd6097d 100644 --- a/src/modules/navigator/mission_feasibility_checker.h +++ b/src/modules/navigator/mission_feasibility_checker.h @@ -61,16 +61,16 @@ 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); + bool checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool home_valid, bool throw_error = false); bool checkMissionItemValidity(dm_item_t dm_current, size_t nMissionItems); /* Checks specific to fixedwing airframes */ - bool checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt); + bool checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid); 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, float home_alt); + bool checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid); public: MissionFeasibilityChecker(); @@ -80,7 +80,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, - float home_alt); + float home_alt, bool home_valid); }; diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 95e3f9f964..782a297fbb 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -134,6 +134,7 @@ public: struct vehicle_gps_position_s* get_gps_position() { return &_gps_pos; } struct sensor_combined_s* get_sensor_combined() { return &_sensor_combined; } struct home_position_s* get_home_position() { return &_home_pos; } + bool home_position_valid() { return _home_position_set; } struct position_setpoint_triplet_s* get_position_setpoint_triplet() { return &_pos_sp_triplet; } struct mission_result_s* get_mission_result() { return &_mission_result; } struct geofence_result_s* get_geofence_result() { return &_geofence_result; }