From 41f535ae262d7a6e5f4a2fe043b66d86dde9993e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 15 Jun 2015 17:03:34 +0200 Subject: [PATCH] navigator: Include distance to first waypoint in mission check, provide warning feedback --- src/modules/navigator/mission.cpp | 82 ++----------- src/modules/navigator/mission.h | 1 - .../navigator/mission_feasibility_checker.cpp | 113 ++++++++++++++---- .../navigator/mission_feasibility_checker.h | 9 +- 4 files changed, 107 insertions(+), 98 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index be27e6208a..4944ebe789 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -79,7 +79,6 @@ Mission::Mission(Navigator *navigator, const char *name) : _mission_type(MISSION_TYPE_NONE), _inited(false), _home_inited(false), - _dist_1wp_ok(false), _missionFeasiblityChecker(), _min_current_sp_distance_xy(FLT_MAX), _mission_item_previous_alt(NAN), @@ -119,7 +118,9 @@ Mission::on_inactive() _navigator->get_mission_result()->valid = _missionFeasiblityChecker.checkMissionFeasible(_navigator->get_mavlink_fd(), _navigator->get_vstatus()->is_rotary_wing, dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(), - _navigator->get_home_position()->alt, _navigator->home_position_valid()); + _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); _navigator->increment_mission_instance_count(); _navigator->set_mission_result_updated(); @@ -255,7 +256,9 @@ Mission::update_offboard_mission() failed = !_missionFeasiblityChecker.checkMissionFeasible(_navigator->get_mavlink_fd(), _navigator->get_vstatus()->is_rotary_wing, dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(), - _navigator->get_home_position()->alt, _navigator->home_position_valid()); + _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); _navigator->get_mission_result()->valid = !failed; _navigator->increment_mission_instance_count(); @@ -308,73 +311,6 @@ Mission::get_absolute_altitude_for_item(struct mission_item_s &mission_item) } } -bool -Mission::check_dist_1wp() -{ - if (_dist_1wp_ok) { - /* always return true after at least one successful check */ - return true; - } - - /* check if first waypoint is not too far from home */ - if (_param_dist_1wp.get() > 0.0f) { - if (_navigator->get_vstatus()->condition_home_position_valid) { - struct mission_item_s mission_item; - - /* find first waypoint (with lat/lon) item in datamanager */ - for (unsigned i = 0; i < _offboard_mission.count; i++) { - if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id), i, - &mission_item, sizeof(mission_item_s)) == sizeof(mission_item_s)) { - - /* check only items with valid lat/lon */ - if ( mission_item.nav_cmd == NAV_CMD_WAYPOINT || - mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || - mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || - mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED || - mission_item.nav_cmd == NAV_CMD_TAKEOFF || - mission_item.nav_cmd == NAV_CMD_PATHPLANNING) { - - /* check distance from current position to item */ - float dist_to_1wp = get_distance_to_next_waypoint( - mission_item.lat, mission_item.lon, - _navigator->get_global_position()->lat, _navigator->get_global_position()->lon); - - if (dist_to_1wp < _param_dist_1wp.get()) { - _dist_1wp_ok = true; - if (dist_to_1wp > ((_param_dist_1wp.get() * 3) / 2)) { - /* allow at 2/3 distance, but warn */ - mavlink_log_critical(_navigator->get_mavlink_fd(), "Warning: First waypoint very far: %d m", (int)dist_to_1wp); - } - return true; - - } else { - /* item is too far from home */ - mavlink_log_critical(_navigator->get_mavlink_fd(), "Waypoint too far: %d m,[MIS_DIST_1WP=%d]", (int)dist_to_1wp, (int)_param_dist_1wp.get()); - return false; - } - } - - } else { - /* error reading, mission is invalid */ - mavlink_log_info(_navigator->get_mavlink_fd(), "error reading offboard mission"); - return false; - } - } - - /* no waypoints found in mission, then we will not fly far away */ - _dist_1wp_ok = true; - return true; - - } else { - mavlink_log_info(_navigator->get_mavlink_fd(), "no home position"); - return false; - } - - } else { - return true; - } -} - void Mission::set_mission_items() { @@ -394,10 +330,8 @@ Mission::set_mission_items() _mission_item_previous_alt = get_absolute_altitude_for_item(_mission_item); } - /* get home distance state */ - bool home_dist_ok = check_dist_1wp(); /* the home dist check provides user feedback, so we initialize it to this */ - bool user_feedback_done = !home_dist_ok; + bool user_feedback_done = false; /* try setting onboard mission item */ if (_param_onboard_enabled.get() && read_mission_item(true, true, &_mission_item)) { @@ -409,7 +343,7 @@ Mission::set_mission_items() _mission_type = MISSION_TYPE_ONBOARD; /* try setting offboard mission item */ - } else if (home_dist_ok && read_mission_item(false, true, &_mission_item)) { + } else if (read_mission_item(false, true, &_mission_item)) { /* if mission type changed, notify */ if (_mission_type != MISSION_TYPE_OFFBOARD) { mavlink_log_info(_navigator->get_mavlink_fd(), "offboard mission now running"); diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 6cfae49598..d77f461574 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -187,7 +187,6 @@ private: bool _inited; bool _home_inited; - bool _dist_1wp_ok; MissionFeasibilityChecker _missionFeasiblityChecker; /**< class that checks if a mission is feasible */ diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index 9d1dc7c7e6..05019bf8aa 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -57,23 +57,40 @@ #endif static const int ERROR = -1; -MissionFeasibilityChecker::MissionFeasibilityChecker() : _mavlink_fd(-1), _capabilities_sub(-1), _initDone(false) +MissionFeasibilityChecker::MissionFeasibilityChecker() : + _mavlink_fd(-1), + _capabilities_sub(-1), + _initDone(false), + _dist_1wp_ok(false) { _nav_caps = {0}; } -bool MissionFeasibilityChecker::checkMissionFeasible(int mavlink_fd, bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid) +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) { bool failed = false; + bool warned = false; /* Init if not done yet */ init(); _mavlink_fd = mavlink_fd; + // first check if we have a valid position + if (!home_valid /* can later use global / local pos for finer granularity */) { + failed = true; + warned = true; + mavlink_log_info(_mavlink_fd, "Not yet ready for mission, no position lock."); + } else { + failed |= !check_dist_1wp(dm_current, nMissionItems, curr_lat, curr_lon, max_waypoint_distance, warning_issued); + } + // check if all mission item commands are supported failed |= !checkMissionItemValidity(dm_current, nMissionItems); - + failed |= !checkGeofence(dm_current, nMissionItems, geofence); + failed |= !checkHomePositionAltitude(dm_current, nMissionItems, home_alt, home_valid, warned); if (isRotarywing) { failed |= !checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence, home_alt, home_valid); @@ -90,28 +107,20 @@ bool MissionFeasibilityChecker::checkMissionFeasible(int mavlink_fd, bool isRota 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, home_valid); - - /* Mission is only marked as feasible if all checks return true */ - return (resGeofence && resHomeAltitude); + /* no custom rotary wing checks yet */ + return true; } 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(); -// 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); /* 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, home_valid); /* Mission is only marked as feasible if all checks return true */ - return (resLanding && resGeofence && resHomeAltitude); + return resLanding; } bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence) @@ -137,7 +146,8 @@ 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 home_valid, bool throw_error) +bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, + float home_alt, bool home_valid, bool &warning_issued, 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++) { @@ -145,17 +155,15 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, const ssize_t len = sizeof(struct mission_item_s); if (dm_read(dm_current, i, &missionitem, len) != len) { + warning_issued = true; /* not supposed to happen unless the datamanager can't access the SD card, etc. */ - if (throw_error) { - return false; - } else { - return true; - } + return false; } /* always reject relative alt without home set */ if (missionitem.altitude_is_relative && !home_valid) { mavlink_log_critical(_mavlink_fd, "Rejecting Mission: No home pos, WP %d uses rel alt", i); + warning_issued = true; return false; } @@ -163,6 +171,9 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, float wp_alt = (missionitem.altitude_is_relative) ? missionitem.altitude + home_alt : missionitem.altitude; if (home_alt > wp_alt) { + + warning_issued = true; + if (throw_error) { mavlink_log_critical(_mavlink_fd, "Rejecting Mission: Waypoint %d below home", i); return false; @@ -275,6 +286,68 @@ bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size return true; } +bool +MissionFeasibilityChecker::check_dist_1wp(dm_item_t dm_current, size_t nMissionItems, double curr_lat, double curr_lon, float dist_first_wp, bool &warning_issued) +{ + if (_dist_1wp_ok) { + /* always return true after at least one successful check */ + return true; + } + + /* check if first waypoint is not too far from home */ + if (dist_first_wp > 0.0f) { + struct mission_item_s mission_item; + + /* find first waypoint (with lat/lon) item in datamanager */ + for (unsigned i = 0; i < nMissionItems; i++) { + if (dm_read(dm_current, i, + &mission_item, sizeof(mission_item_s)) == sizeof(mission_item_s)) { + + /* check only items with valid lat/lon */ + if ( mission_item.nav_cmd == NAV_CMD_WAYPOINT || + mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || + mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || + mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED || + mission_item.nav_cmd == NAV_CMD_TAKEOFF || + mission_item.nav_cmd == NAV_CMD_PATHPLANNING) { + + /* check distance from current position to item */ + float dist_to_1wp = get_distance_to_next_waypoint( + mission_item.lat, mission_item.lon, curr_lat, curr_lon); + + if (dist_to_1wp < dist_first_wp) { + _dist_1wp_ok = true; + if (dist_to_1wp > ((dist_first_wp * 3) / 2)) { + /* allow at 2/3 distance, but warn */ + mavlink_log_critical(_mavlink_fd, "Warning: First waypoint very far: %d m", (int)dist_to_1wp); + warning_issued = true; + } + return true; + + } else { + /* item is too far from home */ + mavlink_log_critical(_mavlink_fd, "Waypoint too far: %d m,[MIS_DIST_1WP=%d]", (int)dist_to_1wp, (int)dist_first_wp); + warning_issued = true; + return false; + } + } + + } else { + /* error reading, mission is invalid */ + mavlink_log_info(_mavlink_fd, "error reading offboard mission"); + return false; + } + } + + /* no waypoints found in mission, then we will not fly far away */ + _dist_1wp_ok = true; + return true; + + } else { + return true; + } +} + void MissionFeasibilityChecker::updateNavigationCapabilities() { (void)orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps); diff --git a/src/modules/navigator/mission_feasibility_checker.h b/src/modules/navigator/mission_feasibility_checker.h index 9c9511be3d..4586f75a47 100644 --- a/src/modules/navigator/mission_feasibility_checker.h +++ b/src/modules/navigator/mission_feasibility_checker.h @@ -57,12 +57,14 @@ private: struct navigation_capabilities_s _nav_caps; bool _initDone; + bool _dist_1wp_ok; void init(); /* 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 home_valid, bool throw_error = false); + bool checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool home_valid, bool &warning_issued, bool throw_error = false); bool checkMissionItemValidity(dm_item_t dm_current, size_t nMissionItems); + bool check_dist_1wp(dm_item_t dm_current, size_t nMissionItems, double curr_lat, double curr_lon, float dist_first_wp, bool &warning_issued); /* Checks specific to fixedwing airframes */ bool checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid); @@ -79,8 +81,9 @@ public: /* * Returns true if mission is feasible and false otherwise */ - bool checkMissionFeasible(int mavlink_fd, bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, - float home_alt, bool home_valid); + 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); };