diff --git a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp b/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp index 7d542bc063..81d8564414 100644 --- a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp +++ b/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * Copyright (c) 2022-2023 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -50,6 +50,7 @@ void FeasibilityChecker::reset() _is_landed = false; _home_alt_msl = NAN; _home_lat_lon = matrix::Vector2d((double)NAN, (double)NAN); + _current_position_lat_lon = matrix::Vector2d((double)NAN, (double)NAN); _vehicle_type = VehicleType::RotaryWing; _mission_validity_failed = false; @@ -119,6 +120,12 @@ void FeasibilityChecker::updateData() _is_landed = land_detected.landed; } + if (_vehicle_global_position_sub.updated()) { + vehicle_global_position_s vehicle_global_position = {}; + _vehicle_global_position_sub.copy(&vehicle_global_position); + _current_position_lat_lon = matrix::Vector2d(vehicle_global_position.lat, vehicle_global_position.lon); + } + param_t handle = param_find("FW_LND_ANG"); if (handle != PARAM_INVALID) { @@ -189,7 +196,7 @@ void FeasibilityChecker::doCommonChecks(mission_item_s &mission_item, const int } if (!_distance_first_waypoint_failed) { - _distance_first_waypoint_failed = !checkDistanceToFirstWaypoint(mission_item); + _distance_first_waypoint_failed = !checkHorizontalDistanceToFirstWaypoint(mission_item); } if (!_below_home_alt_failed) { @@ -595,21 +602,29 @@ bool FeasibilityChecker::checkTakeoffLandAvailable() } -bool FeasibilityChecker::checkDistanceToFirstWaypoint(mission_item_s &mission_item) +bool FeasibilityChecker::checkHorizontalDistanceToFirstWaypoint(mission_item_s &mission_item) { - if (_param_mis_dist_1wp <= 0.0f || !_home_lat_lon.isAllFinite()) { - /* param not set, check is ok */ - return true; + matrix::Vector2d position_reference = matrix::Vector2d((double)NAN, (double)NAN); + + // take last known vehicle global_position, or Home position if not available + if (_current_position_lat_lon.isAllFinite()) { + position_reference = _current_position_lat_lon; + + } else if (_home_lat_lon.isAllFinite()) { + position_reference = _home_lat_lon; } - if (!_first_waypoint_found && MissionBlock::item_contains_position(mission_item)) { + if (_param_mis_dist_1wp > FLT_EPSILON && + position_reference.isAllFinite() && + !_first_waypoint_found && + MissionBlock::item_contains_position(mission_item)) { + _first_waypoint_found = true; - /* check distance from current position to item */ - float dist_to_1wp = get_distance_to_next_waypoint( - mission_item.lat, mission_item.lon, - _home_lat_lon(0), _home_lat_lon(1)); + const float dist_to_1wp = get_distance_to_next_waypoint( + mission_item.lat, mission_item.lon, + position_reference(0), position_reference(1)); if (dist_to_1wp < _param_mis_dist_1wp) { @@ -627,7 +642,6 @@ bool FeasibilityChecker::checkDistanceToFirstWaypoint(mission_item_s &mission_it } } - /* no waypoints found in mission, then we will not fly far away */ return true; } diff --git a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp b/src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp index 02e1a0cb5f..a6dff6f89a 100644 --- a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp +++ b/src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * Copyright (c) 2022-2023 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -37,6 +37,7 @@ #include #include #include +#include #include #include #include @@ -96,6 +97,7 @@ private: uORB::Subscription _home_pos_sub{ORB_ID(home_position)}; uORB::Subscription _status_sub{ORB_ID(vehicle_status)}; uORB::Subscription _land_detector_sub{ORB_ID(vehicle_land_detected)}; + uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)}; // parameters float _param_fw_lnd_ang{0.f}; @@ -106,6 +108,7 @@ private: bool _is_landed{false}; float _home_alt_msl{NAN}; matrix::Vector2d _home_lat_lon = matrix::Vector2d((double)NAN, (double)NAN); + matrix::Vector2d _current_position_lat_lon = matrix::Vector2d((double)NAN, (double)NAN); VehicleType _vehicle_type{VehicleType::RotaryWing}; // internal flags to keep track of which checks failed @@ -129,7 +132,7 @@ private: int _landing_approach_index{-1}; mission_item_s _mission_item_previous = {}; - // internal checkDistanceToFirstWaypoint variables + // internal checkHorizontalDistanceToFirstWaypoint variables bool _first_waypoint_found{false}; // internal checkDistancesBetweenWaypoints variables @@ -171,12 +174,13 @@ private: bool checkLandPatternValidity(mission_item_s &mission_item, const int current_index, const int last_index); /** - * @brief Check distance to first waypoint. + * @brief Check distance to first waypoint from current vehicle position. + * Use Home position instead of vehicle position if vehicle position is invalid. * * @param mission_item The current mission item * @return False if the check failed. */ - bool checkDistanceToFirstWaypoint(mission_item_s &mission_item); + bool checkHorizontalDistanceToFirstWaypoint(mission_item_s &mission_item); /** * @brief Check distances between waypoints diff --git a/src/modules/navigator/MissionFeasibility/FeasibilityCheckerTest.cpp b/src/modules/navigator/MissionFeasibility/FeasibilityCheckerTest.cpp index f43a609586..d750958ed4 100644 --- a/src/modules/navigator/MissionFeasibility/FeasibilityCheckerTest.cpp +++ b/src/modules/navigator/MissionFeasibility/FeasibilityCheckerTest.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2022 PX4 Development Team. All rights reserved. + * Copyright (C) 2022-2023 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -33,8 +33,6 @@ #include #include "FeasibilityChecker.hpp" -#include -#include #include @@ -67,6 +65,15 @@ public: orb_publish(ORB_ID(home_position), home_pub, &home); } + void publishCurrentPosition(double lat, double lon) + { + vehicle_global_position_s gpos = {}; + gpos.lat = lat; + gpos.lon = lon; + orb_advert_t gpos_pub = orb_advertise(ORB_ID(vehicle_global_position), &gpos); + orb_publish(ORB_ID(vehicle_global_position), gpos_pub, &gpos); + } + void publishLanded(bool landed) { vehicle_land_detected_s land_detected = {}; @@ -125,35 +132,72 @@ TEST_F(FeasibilityCheckerTest, mission_item_validity) TEST_F(FeasibilityCheckerTest, check_dist_first_waypoint) { + // GIVEN: MIS_DIST_1WP set to 500m TestFeasibilityChecker checker; checker.publishLanded(true); - checker.publishHomePosition(0, 0, 0); - param_t param = param_handle(px4::params::MIS_DIST_1WP); - float max_dist = 500.0f; param_set(param, &max_dist); checker.paramsChanged(); - - mission_item_s mission_item = {}; mission_item.nav_cmd = NAV_CMD_WAYPOINT; double lat_new, lon_new; + + // GIVEN: no valid Current position, no valid Home + + + // THEN: always pass + checker.processNextItem(mission_item, 0, 1); + ASSERT_EQ(checker.someCheckFailed(), false); + + // GIVEN: no valid current position, but valid Home. First WP 501m away from Home + checker.reset(); + checker.publishLanded(true); + checker.publishHomePosition(0, 0, 0); waypoint_from_heading_and_distance(mission_item.lat, mission_item.lon, 0, 501, &lat_new, &lon_new); mission_item.lat = lat_new; mission_item.lon = lon_new; - checker.processNextItem(mission_item, 0, 1); + // THEN: check should fail + checker.processNextItem(mission_item, 0, 1); ASSERT_EQ(checker.someCheckFailed(), true); + // BUT WHEN: no valid current position, but valid Home. First WP 499m away from Home checker.reset(); checker.publishLanded(true); checker.publishHomePosition(0, 0, 0); waypoint_from_heading_and_distance(0, 0, 0, 499, &lat_new, &lon_new); mission_item.lat = lat_new; mission_item.lon = lon_new; - checker.processNextItem(mission_item, 0, 1); + // THEN: pass + checker.processNextItem(mission_item, 0, 1); + ASSERT_EQ(checker.someCheckFailed(), false); + + // BUT WHEN: valid current position, valid Home, first WP 501m away from current pos + checker.reset(); + checker.publishLanded(true); + checker.publishHomePosition(0, 0, 0); + checker.publishCurrentPosition(0, 0); + waypoint_from_heading_and_distance(0, 0, 0, 501, &lat_new, &lon_new); + mission_item.lat = lat_new; + mission_item.lon = lon_new; + + // THEN: fail + checker.processNextItem(mission_item, 0, 1); + ASSERT_EQ(checker.someCheckFailed(), true); + + // BUT WHEN: valid current position, valid Home, fist WP 499m away from current but more from Home + checker.reset(); + checker.publishLanded(true); + checker.publishHomePosition(10, 20, 0); // random position far away + checker.publishCurrentPosition(0, 0); + waypoint_from_heading_and_distance(0, 0, 0, 499, &lat_new, &lon_new); + mission_item.lat = lat_new; + mission_item.lon = lon_new; + + // THEN: pass + checker.processNextItem(mission_item, 0, 1); ASSERT_EQ(checker.someCheckFailed(), false); }