diff --git a/boards/corvon/743v1/default.px4board b/boards/corvon/743v1/default.px4board index 10912bcfda8..1fa3c562b4e 100644 --- a/boards/corvon/743v1/default.px4board +++ b/boards/corvon/743v1/default.px4board @@ -57,6 +57,7 @@ CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y CONFIG_MODULES_NAVIGATOR=y CONFIG_NAVIGATOR_GEOFENCE_AVOIDANCE=n +# CONFIG_NAVIGATOR_GEOFENCE_PATH_CHECKS is not set CONFIG_MODULES_PAYLOAD_DELIVERER=y CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_SENSORS=y diff --git a/boards/diatone/mamba-f405-mk2/default.px4board b/boards/diatone/mamba-f405-mk2/default.px4board index 050dece171f..17cbf1d258a 100644 --- a/boards/diatone/mamba-f405-mk2/default.px4board +++ b/boards/diatone/mamba-f405-mk2/default.px4board @@ -45,6 +45,7 @@ CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y CONFIG_MODULES_NAVIGATOR=y # CONFIG_NAVIGATOR_ADSB is not set +# CONFIG_NAVIGATOR_GEOFENCE_PATH_CHECKS is not set CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_SENSORS=y CONFIG_SYSTEMCMDS_DMESG=y diff --git a/boards/px4/fmu-v2/default.px4board b/boards/px4/fmu-v2/default.px4board index ec8b1463b26..1b5c9815d16 100644 --- a/boards/px4/fmu-v2/default.px4board +++ b/boards/px4/fmu-v2/default.px4board @@ -44,6 +44,7 @@ CONFIG_MODULES_MC_RATE_CONTROL=y CONFIG_MODULES_NAVIGATOR=y # CONFIG_NAVIGATOR_ADSB is not set # CONFIG_NAVIGATOR_GEOFENCE_AVOIDANCE is not set +# CONFIG_NAVIGATOR_GEOFENCE_PATH_CHECKS is not set CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_SENSORS=y # CONFIG_SENSORS_VEHICLE_AIRSPEED is not set diff --git a/src/modules/navigator/Kconfig b/src/modules/navigator/Kconfig index 5613009af8f..cdbb7ae92f7 100644 --- a/src/modules/navigator/Kconfig +++ b/src/modules/navigator/Kconfig @@ -46,6 +46,15 @@ menuconfig NAVIGATOR_GEOFENCE_AVOIDANCE Add support for Return mode to route around geofences. The node budget (and thus RAM usage) is set at runtime with GF_AVOID_NODES. +menuconfig NAVIGATOR_GEOFENCE_PATH_CHECKS + bool "Check the mission flight path against the geofence at upload" + default y + depends on MODULES_NAVIGATOR + ---help--- + Reject an uploaded mission whose flight path crosses a geofence, not only a + mission with a waypoint outside it. Boards without the flash for it keep the + waypoint-only check. + menuconfig NAVIGATOR_ADSB_FAKE_TRAFFIC bool "Include fake-traffic injection for DAA testing" default n diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 699a0a16554..c00c0cc9b9a 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -845,6 +845,15 @@ bool Geofence::isBelowMaxAltitude(float altitude) return inside_fence; } +bool Geofence::isWithinAltitudeBand(float altitude) +{ + if (!isEmpty() && _altitude_max > _altitude_min) { + return !(altitude > _altitude_max || altitude < _altitude_min); + } + + return true; +} + bool Geofence::isInsidePolygonOrCircle(double lat, double lon, float altitude) { if (isEmpty()) { @@ -853,10 +862,8 @@ bool Geofence::isInsidePolygonOrCircle(double lat, double lon, float altitude) } /* Vertical check */ - if (_altitude_max > _altitude_min) { // only enable vertical check if configured properly - if (altitude > _altitude_max || altitude < _altitude_min) { - return false; - } + if (!isWithinAltitudeBand(altitude)) { + return false; } /* Horizontal check: iterate all polygons & circles */ diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h index cc3ecde8d5a..d0720a334e7 100644 --- a/src/modules/navigator/geofence.h +++ b/src/modules/navigator/geofence.h @@ -119,6 +119,13 @@ public: * On failure, results are false when the output buffer and count are valid. */ bool checkPathBatch(const PathCheck *paths, size_t num_paths, bool *results); + bool isReadyForPathChecks() const { return _path_check_ready; } + + /** True while a fence update is loading or a failed load waits for its retry. */ + bool isFenceUpdatePending() const + { + return _initiate_fence_updated || _dataman_state != DatamanState::UpdateRequestWait || _fence_retry_time != 0; + } /** * @brief check if the horizontal distance to Home is greater than the maximum allowed distance @@ -135,6 +142,9 @@ public: */ bool isBelowMaxAltitude(float altitude); + /** Check the altitude band from a loaded fence file, if configured. */ + bool isWithinAltitudeBand(float altitude); + virtual bool isInsidePolygonOrCircle(double lat, double lon, float altitude); bool valid(); diff --git a/src/modules/navigator/mission_base.cpp b/src/modules/navigator/mission_base.cpp index c5ad050ab4f..6bf9a6a8c78 100644 --- a/src/modules/navigator/mission_base.cpp +++ b/src/modules/navigator/mission_base.cpp @@ -784,12 +784,31 @@ MissionBase::checkMissionRestart() void MissionBase::check_mission_valid(bool forced) { - // Allow forcing it, since we currently not rechecking if parameters have changed. - if (forced || - (_navigator->get_mission_result()->mission_id != _mission.mission_id) || - (_navigator->get_mission_result()->geofence_id != _mission.geofence_id) || - (_navigator->get_mission_result()->home_position_counter != _navigator->get_home_position()->update_count)) { + const bool mission_changed = _navigator->get_mission_result()->mission_id != _mission.mission_id; + const bool inputs_changed = mission_changed + || (_navigator->get_mission_result()->geofence_id != _mission.geofence_id) + || (_navigator->get_mission_result()->home_position_counter != _navigator->get_home_position()->update_count); + if (_navigator->get_geofence().isFenceUpdatePending()) { + // A fence upload reloads the fence for about a second. Keep the last verdict until then + // and recheck when it is ready: reporting the mission invalid now would make Commander + // treat it as missing and fail over out of Mission mode. + _mission_checked = false; + _mission_check_pending = true; + + // A mission that was never checked has no verdict to keep. + if (mission_changed) { + _navigator->get_mission_result()->valid = false; + _navigator->set_mission_result_updated(); + } + + return; + } + + // Allow forcing it, since we currently not rechecking if parameters have changed. + if (forced || _mission_check_pending || inputs_changed) { + + _mission_check_pending = false; _navigator->get_mission_result()->mission_id = _mission.mission_id; _navigator->get_mission_result()->geofence_id = _mission.geofence_id; _navigator->get_mission_result()->home_position_counter = _navigator->get_home_position()->update_count; @@ -1583,6 +1602,7 @@ bool MissionBase::canRunMissionFeasibility() { return _navigator->home_global_position_valid() && // Need to have a home position checked _navigator->get_global_position()->timestamp > 0 && // Need to have a position, for first waypoint check + _navigator->get_geofence().isReadyForPathChecks() && (_geofence_status_sub.get().timestamp > 0) && // Geofence data must be loaded (_geofence_status_sub.get().geofence_id == _mission.geofence_id) && (_geofence_status_sub.get().status == geofence_status_s::GF_STATUS_READY); diff --git a/src/modules/navigator/mission_base.h b/src/modules/navigator/mission_base.h index ff4f32e424c..7c56601b5cc 100644 --- a/src/modules/navigator/mission_base.h +++ b/src/modules/navigator/mission_base.h @@ -442,6 +442,7 @@ protected: bool _is_current_planned_mission_item_valid{false}; /**< Flag indicating if the currently loaded mission item is valid*/ bool _mission_has_been_activated{false}; /**< Flag indicating if the mission has been activated*/ bool _mission_checked{false}; /**< Flag indicating if the mission has been checked by the mission validator*/ + bool _mission_check_pending{false}; /**< Retry a check deferred while the geofence was unavailable. */ bool _system_disarmed_while_inactive{false}; /**< Flag indicating if the system has been disarmed while mission is inactive*/ mission_s _mission; /**< Currently active mission*/ float _mission_init_climb_altitude_amsl{NAN}; /**< altitude AMSL the vehicle will climb to when mission starts */ diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index 6933667b37d..20e28fcce15 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -109,52 +109,179 @@ MissionFeasibilityChecker::checkMissionFeasible(const mission_s &mission) bool MissionFeasibilityChecker::checkMissionAgainstGeofence(const mission_s &mission, float home_alt, bool home_valid) { - if (_navigator->get_geofence().isHomeRequired() && !home_valid) { + Geofence &geofence = _navigator->get_geofence(); + + if (geofence.isHomeRequired() && !home_valid) { mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Geofence requires valid home position\t"); events::send(events::ID("navigator_mis_geofence_no_home"), {events::Log::Error, events::LogInternal::Info}, "Geofence requires a valid home position"); return false; } - /* Check if all mission items are inside the geofence (if we have a valid geofence) */ - if (_navigator->get_geofence().valid()) { - for (size_t i = 0; i < mission.count; i++) { - struct mission_item_s missionitem = {}; + if (!geofence.isReadyForPathChecks()) { + logGeofenceUnavailable(); + return false; + } - bool success = _dataman_client.readSync((dm_item_t)mission.mission_dataman_id, i, - reinterpret_cast(&missionitem), - sizeof(mission_item_s)); + if (!geofence.valid()) { + return true; + } - if (!success) { - /* not supposed to happen unless the datamanager can't access the SD card, etc. */ - logDatamanReadFailure(i, mission.mission_dataman_id); - return false; + enum class Failure { None, DatamanRead, NoHome, Waypoint } failure = Failure::None; + size_t failed_item = 0; +#if defined(CONFIG_NAVIGATOR_GEOFENCE_PATH_CHECKS) + /* Check mission positions and the paths between them. */ + static GeofencePathBatch batch{}; // keep the shared batch off the stack. + batch.count = 0; + matrix::Vector2d previous_position{}; + bool have_previous_position = false; +#endif // CONFIG_NAVIGATOR_GEOFENCE_PATH_CHECKS + + for (size_t i = 0; i < mission.count; i++) { + struct mission_item_s missionitem = {}; + + bool success = _dataman_client.readSync((dm_item_t)mission.mission_dataman_id, i, + reinterpret_cast(&missionitem), + sizeof(mission_item_s)); + + if (!success) { + failure = Failure::DatamanRead; + failed_item = i; + break; + } + + if (!mission_item_contains_position(missionitem)) { + continue; + } + + if (missionitem.altitude_is_relative && !home_valid) { + failure = Failure::NoHome; + failed_item = i; + break; + } + + // Geofence function checks against home altitude amsl + missionitem.altitude = missionitem.altitude_is_relative ? missionitem.altitude + home_alt : missionitem.altitude; + + bool point_valid = PX4_ISFINITE(missionitem.lat) && PX4_ISFINITE(missionitem.lon) + && PX4_ISFINITE(missionitem.altitude) + && fabs(missionitem.lat) <= 90.0 && fabs(missionitem.lon) <= 180.0; + +#if defined(CONFIG_NAVIGATOR_GEOFENCE_PATH_CHECKS) + + if (point_valid) { + if (!have_previous_position) { + // Check polygon membership once; the paths check all later positions. + point_valid = geofence.checkPointAgainstAllGeofences(missionitem.lat, missionitem.lon, missionitem.altitude); + + } else { + // Home-distance and altitude limits are separate from the horizontal fence shapes. + point_valid = geofence.isCloserThanMaxDistToHome(missionitem.lat, missionitem.lon, missionitem.altitude) + && geofence.isBelowMaxAltitude(missionitem.altitude) + && geofence.isWithinAltitudeBand(missionitem.altitude); } + } - if (missionitem.altitude_is_relative && !home_valid) { - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Geofence requires valid home position\t"); - events::send(events::ID("navigator_mis_geofence_no_home2"), {events::Log::Error, events::LogInternal::Info}, - "Geofence requires a valid home position"); - return false; - } +#else + // Without path checks every position is checked against the whole fence. + point_valid = point_valid && geofence.checkPointAgainstAllGeofences(missionitem.lat, missionitem.lon, + missionitem.altitude); +#endif // CONFIG_NAVIGATOR_GEOFENCE_PATH_CHECKS - // Geofence function checks against home altitude amsl - missionitem.altitude = missionitem.altitude_is_relative ? missionitem.altitude + home_alt : missionitem.altitude; + if (!point_valid) { + failure = Failure::Waypoint; + failed_item = i; + break; + } - if (mission_item_contains_position(missionitem) && !_navigator->get_geofence().checkPointAgainstAllGeofences( - missionitem.lat, missionitem.lon, missionitem.altitude)) { +#if defined(CONFIG_NAVIGATOR_GEOFENCE_PATH_CHECKS) + const matrix::Vector2d position {missionitem.lat, missionitem.lon}; - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Geofence violation for waypoint %zu\t", i + 1); - events::send(events::ID("navigator_mis_geofence_violation"), {events::Log::Error, events::LogInternal::Info}, - "Geofence violation for waypoint {1}", - i + 1); - return false; - } + if (!have_previous_position) { + // A zero-length path also rejects a lone waypoint on a fence boundary. + previous_position = position; + have_previous_position = true; + } + + batch.paths[batch.count] = {previous_position, position}; + batch.mission_indices[batch.count++] = static_cast(i); + previous_position = position; + + if (batch.count == kGeofencePathBatchSize && !checkGeofencePathBatch(batch)) { + return false; + } + +#endif // CONFIG_NAVIGATOR_GEOFENCE_PATH_CHECKS + } + +#if defined(CONFIG_NAVIGATOR_GEOFENCE_PATH_CHECKS) + + // Report an earlier buffered path breach before the current item's failure. + if (!checkGeofencePathBatch(batch)) { + return false; + } + +#endif // CONFIG_NAVIGATOR_GEOFENCE_PATH_CHECKS + + switch (failure) { + case Failure::None: + return true; + + case Failure::DatamanRead: + logDatamanReadFailure(failed_item, mission.mission_dataman_id); + break; + + case Failure::NoHome: + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Geofence requires valid home position\t"); + events::send(events::ID("navigator_mis_geofence_no_home2"), {events::Log::Error, events::LogInternal::Info}, + "Geofence requires a valid home position"); + break; + + case Failure::Waypoint: + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Geofence violation for waypoint %zu\t", failed_item + 1); + events::send(events::ID("navigator_mis_geofence_violation"), {events::Log::Error, events::LogInternal::Info}, + "Geofence violation for waypoint {1}", failed_item + 1); + break; + } + + return false; +} + +#if defined(CONFIG_NAVIGATOR_GEOFENCE_PATH_CHECKS) +bool MissionFeasibilityChecker::checkGeofencePathBatch(GeofencePathBatch &batch) +{ + if (batch.count == 0) { + return true; + } + + if (!_navigator->get_geofence().checkPathBatch(batch.paths, batch.count, batch.results)) { + logGeofenceUnavailable(); + return false; + } + + for (size_t i = 0; i < batch.count; ++i) { + if (!batch.results[i]) { + const uint16_t waypoint = batch.mission_indices[i] + 1; + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Geofence breach on path to waypoint %u\t", + static_cast(waypoint)); + events::send(events::ID("navigator_mis_geofence_path_violation"), + {events::Log::Error, events::LogInternal::Info}, + "Geofence breach on path to waypoint {1}", waypoint); + return false; } } + batch.count = 0; return true; } +#endif // CONFIG_NAVIGATOR_GEOFENCE_PATH_CHECKS + +void MissionFeasibilityChecker::logGeofenceUnavailable() +{ + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: geofence path check unavailable\t"); + events::send(events::ID("navigator_mis_geofence_unavailable"), {events::Log::Error, events::LogInternal::Info}, + "Mission rejected: geofence path check unavailable"); +} void MissionFeasibilityChecker::logDatamanReadFailure(const size_t mission_item, const uint8_t dataman_id) { diff --git a/src/modules/navigator/mission_feasibility_checker.h b/src/modules/navigator/mission_feasibility_checker.h index 88b36b6f9b5..ea82cf9f04a 100644 --- a/src/modules/navigator/mission_feasibility_checker.h +++ b/src/modules/navigator/mission_feasibility_checker.h @@ -46,17 +46,32 @@ #include #include #include "MissionFeasibility/FeasibilityChecker.hpp" +#include "geofence.h" class Navigator; class MissionFeasibilityChecker: public ModuleParams { private: +#if defined(CONFIG_NAVIGATOR_GEOFENCE_PATH_CHECKS) + static constexpr size_t kGeofencePathBatchSize = Geofence::MAX_PATH_CHECKS; + struct GeofencePathBatch { + Geofence::PathCheck paths[kGeofencePathBatchSize]; + uint16_t mission_indices[kGeofencePathBatchSize]; + bool results[kGeofencePathBatchSize]; + size_t count{0}; + }; +#endif // CONFIG_NAVIGATOR_GEOFENCE_PATH_CHECKS + Navigator *_navigator{nullptr}; DatamanClient &_dataman_client; FeasibilityChecker _feasibility_checker; bool checkMissionAgainstGeofence(const mission_s &mission, float home_alt, bool home_valid); +#if defined(CONFIG_NAVIGATOR_GEOFENCE_PATH_CHECKS) + bool checkGeofencePathBatch(GeofencePathBatch &batch); +#endif // CONFIG_NAVIGATOR_GEOFENCE_PATH_CHECKS + void logGeofenceUnavailable(); void logDatamanReadFailure(const size_t mission_item, const uint8_t dataman_id); public: diff --git a/src/modules/navigator/test/CMakeLists.txt b/src/modules/navigator/test/CMakeLists.txt index bed0a623a4a..e07e7c6962e 100644 --- a/src/modules/navigator/test/CMakeLists.txt +++ b/src/modules/navigator/test/CMakeLists.txt @@ -37,6 +37,9 @@ px4_add_functional_gtest(SRC test_geofence.cpp LINKLIBS modules__navigator modul px4_add_functional_gtest(SRC test_mission_base.cpp LINKLIBS modules__navigator modules__dataman) px4_add_functional_gtest(SRC test_RTL.cpp LINKLIBS modules__navigator modules__dataman) px4_add_functional_gtest(SRC test_geofence_paths.cpp LINKLIBS modules__navigator modules__dataman) +if(CONFIG_NAVIGATOR_GEOFENCE_PATH_CHECKS) + px4_add_functional_gtest(SRC test_mission_feasibility_geofence.cpp LINKLIBS modules__navigator modules__dataman) +endif() px4_add_functional_gtest(SRC test_mission_route_cache.cpp LINKLIBS modules__navigator navigator_mission_route modules__dataman) if(CONFIG_MODULES_VISION_TARGET_ESTIMATOR) diff --git a/src/modules/navigator/test/test_mission_base.cpp b/src/modules/navigator/test/test_mission_base.cpp index 57244aaa66a..442affafc8e 100644 --- a/src/modules/navigator/test/test_mission_base.cpp +++ b/src/modules/navigator/test/test_mission_base.cpp @@ -44,6 +44,7 @@ #include "mission_base.h" #include "navigator.h" +#include "support/geofence_test_helpers.h" #include "support/mission_route_cache_test_peer.h" #include "support/navigator_dataman_test.h" #include "support/vector_mission_item_store.h" @@ -52,7 +53,10 @@ #include #include +#include +#include #include +#include #include class MissionBaseTestPeer : public MissionBase @@ -119,6 +123,15 @@ public: return _mission.current_seq; } + void checkMission(bool forced = false) + { + // Normal MissionBase callers mark the check before calling the validator. + _mission_checked = true; + check_mission_valid(forced); + } + + bool missionChecked() const { return _mission_checked; } + using MissionBase::findNextPositionIndex; using MissionBase::findPreviousPositionIndex; using MissionBase::getNonJumpItem; @@ -186,6 +199,224 @@ protected: IgnoreDoJumpMissionBaseTestPeer mission_base{}; }; +class MissionBaseTestNavigator : public Navigator +{ +public: + using Navigator::updateParams; +}; + +class MissionBaseFeasibilityTest : public NavigatorDatamanTestBase +{ +protected: + void SetUp() override + { + param_reset_all(); + _navigator.updateParams(); + ASSERT_TRUE(_dataman_client.clearSync(DM_KEY_WAYPOINTS_OFFBOARD_0)); + ASSERT_TRUE(_dataman_client.clearSync(DM_KEY_FENCE_POINTS_0)); + + home_position_s *home = _navigator.get_home_position(); + home->timestamp = hrt_absolute_time(); + home->lat = kBaseLat; + home->lon = kBaseLon; + home->alt = 0.f; + home->valid_hpos = true; + home->valid_alt = true; + _home_pub.publish(*home); + _navigator.get_land_detected()->landed = true; + + vehicle_status_s status{}; + status.timestamp = hrt_absolute_time(); + status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING; + *_navigator.get_vstatus() = status; + _status_pub.publish(status); + + mission_fence_point_s circle{}; + circle.nav_cmd = NAV_CMD_FENCE_CIRCLE_INCLUSION; + circle.frame = NAV_FRAME_GLOBAL; + circle.lat = kBaseLat; + circle.lon = kBaseLon; + circle.circle_radius = 100.f; + ASSERT_TRUE(_dataman_client.writeSync(DM_KEY_FENCE_POINTS_0, 0, + reinterpret_cast(&circle), sizeof(circle))); + mission_stats_entry_s stats{}; + stats.dataman_id = DM_KEY_FENCE_POINTS_0; + stats.opaque_id = 1; + stats.num_items = 1; + ASSERT_TRUE(_dataman_client.writeSync(DM_KEY_FENCE_POINTS_STATE, 0, + reinterpret_cast(&stats), sizeof(stats))); + _navigator.get_geofence().updateFence(); + ASSERT_TRUE(finishFenceUpdate()); + + mission_s mission{}; + mission.timestamp = hrt_absolute_time(); + mission.mission_id = 1; + mission.geofence_id = stats.opaque_id; + mission.mission_dataman_id = DM_KEY_WAYPOINTS_OFFBOARD_0; + _mission_base.loadTestMission({makePositionItem(kBaseLat, kBaseLon, kAlt)}, mission); + ASSERT_TRUE(writeWaypoint(0.f)); + } + + void TearDown() override { param_reset_all(); } + + bool finishFenceUpdate() + { + Geofence &fence = _navigator.get_geofence(); + const hrt_abstime start = hrt_absolute_time(); + + while (!fence.isReadyForPathChecks() && hrt_elapsed_time(&start) < 1_s) { + fence.run(); + px4_usleep(1000); + } + + return fence.isReadyForPathChecks(); + } + + bool writeWaypoint(float north_m) + { + mission_item_s item = makePositionItem(kBaseLat, kBaseLon, kAlt); + add_vector_to_global_position(kBaseLat, kBaseLon, north_m, 0.f, &item.lat, &item.lon); + return _dataman_client.writeSync(DM_KEY_WAYPOINTS_OFFBOARD_0, 0, + reinterpret_cast(&item), sizeof(item)); + } + + DatamanClient _dataman_client{}; + MissionBaseTestNavigator _navigator{}; + MissionBaseTestPeer _mission_base{&_navigator}; + uORB::Publication _home_pub{ORB_ID(home_position)}; + uORB::Publication _status_pub{ORB_ID(vehicle_status)}; +}; + +TEST_F(MissionBaseFeasibilityTest, DeferredCheckPreservesVerdictUntilRetry) +{ + _mission_base.checkMission(true); + const mission_result_s checked_result = *_navigator.get_mission_result(); + ASSERT_TRUE(checked_result.valid); + + // Change storage without changing IDs to verify that the pending check really runs. + ASSERT_TRUE(writeWaypoint(200.f)); + _navigator.get_geofence().updateFence(); + _mission_base.checkMission(true); + EXPECT_TRUE(_navigator.get_mission_result()->valid); + EXPECT_FALSE(_mission_base.missionChecked()); + EXPECT_EQ(_navigator.get_mission_result()->mission_id, checked_result.mission_id); + EXPECT_EQ(_navigator.get_mission_result()->geofence_id, checked_result.geofence_id); + EXPECT_EQ(_navigator.get_mission_result()->home_position_counter, checked_result.home_position_counter); + + ASSERT_TRUE(finishFenceUpdate()); + _mission_base.checkMission(); + ASSERT_FALSE(_navigator.get_mission_result()->valid); + EXPECT_TRUE(_mission_base.missionChecked()); + + // A completed failure stays cached until another check is requested. + ASSERT_TRUE(writeWaypoint(0.f)); + _mission_base.checkMission(); + EXPECT_FALSE(_navigator.get_mission_result()->valid); + + _navigator.get_geofence().updateFence(); + _mission_base.checkMission(true); + EXPECT_FALSE(_navigator.get_mission_result()->valid); + EXPECT_FALSE(_mission_base.missionChecked()); + ASSERT_TRUE(finishFenceUpdate()); + _mission_base.checkMission(); + EXPECT_TRUE(_navigator.get_mission_result()->valid); + EXPECT_TRUE(_mission_base.missionChecked()); +} + +TEST_F(MissionBaseFeasibilityTest, SpentFenceRetriesLetTheCheckReject) +{ + _mission_base.checkMission(true); + ASSERT_TRUE(_navigator.get_mission_result()->valid); + Geofence &fence = _navigator.get_geofence(); + fence.updateFence(); + + for (unsigned attempt = 0; attempt <= GeofenceTestPeer::maxLoadRetries(); ++attempt) { + SCOPED_TRACE(attempt); + // The verdict is kept while a retry is still pending. + _mission_base.checkMission(true); + EXPECT_TRUE(_navigator.get_mission_result()->valid); + EXPECT_FALSE(_mission_base.missionChecked()); + GeofenceTestPeer::expireRetryDelay(fence); + fence.run(); + fence.run(); + ASSERT_TRUE(GeofenceTestPeer::failPendingRead(fence)); + fence.run(); + fence.run(); + } + + // Once the fence gives up, the deferred check runs and rejects the mission. + ASSERT_FALSE(fence.isFenceUpdatePending()); + _mission_base.checkMission(true); + EXPECT_FALSE(_navigator.get_mission_result()->valid); + EXPECT_TRUE(_mission_base.missionChecked()); +} + +enum class MissionFeasibilityInput { Mission, Geofence, Home }; + +struct ChangedMissionInput { + const char *name; + MissionFeasibilityInput input; + bool keeps_verdict; +}; + +class MissionBaseChangedInputTest : public MissionBaseFeasibilityTest, + public ::testing::WithParamInterface {}; + +TEST_P(MissionBaseChangedInputTest, DeferredCheckInvalidatesOnlyAnUncheckedMission) +{ + _mission_base.checkMission(true); + const mission_result_s checked_result = *_navigator.get_mission_result(); + ASSERT_TRUE(checked_result.valid); + + mission_s mission{}; + mission.timestamp = hrt_absolute_time(); + mission.mission_id = checked_result.mission_id; + mission.geofence_id = checked_result.geofence_id; + mission.mission_dataman_id = DM_KEY_WAYPOINTS_OFFBOARD_0; + + switch (GetParam().input) { + case MissionFeasibilityInput::Mission: + ++mission.mission_id; + break; + + case MissionFeasibilityInput::Geofence: + ++mission.geofence_id; + break; + + case MissionFeasibilityInput::Home: + ++_navigator.get_home_position()->update_count; + break; + } + + _mission_base.loadTestMission({makePositionItem(kBaseLat, kBaseLon, kAlt)}, mission); + _navigator.get_geofence().updateFence(); + _mission_base.checkMission(true); + // A fence or Home change keeps the checked mission flyable until the recheck. + EXPECT_EQ(_navigator.get_mission_result()->valid, GetParam().keeps_verdict); + EXPECT_FALSE(_mission_base.missionChecked()); + // No result has been computed for the changed inputs yet. + EXPECT_EQ(_navigator.get_mission_result()->mission_id, checked_result.mission_id); + EXPECT_EQ(_navigator.get_mission_result()->geofence_id, checked_result.geofence_id); + EXPECT_EQ(_navigator.get_mission_result()->home_position_counter, checked_result.home_position_counter); + + ASSERT_TRUE(finishFenceUpdate()); + _mission_base.checkMission(); + EXPECT_TRUE(_navigator.get_mission_result()->valid); + EXPECT_TRUE(_mission_base.missionChecked()); + EXPECT_EQ(_navigator.get_mission_result()->mission_id, mission.mission_id); + EXPECT_EQ(_navigator.get_mission_result()->geofence_id, mission.geofence_id); + EXPECT_EQ(_navigator.get_mission_result()->home_position_counter, _navigator.get_home_position()->update_count); +} + +INSTANTIATE_TEST_SUITE_P(ChangedInputs, MissionBaseChangedInputTest, ::testing::Values( + ChangedMissionInput{"Mission", MissionFeasibilityInput::Mission, false}, + ChangedMissionInput{"Geofence", MissionFeasibilityInput::Geofence, true}, + ChangedMissionInput{"Home", MissionFeasibilityInput::Home, true}), + [](const ::testing::TestParamInfo &test_info) +{ + return test_info.param.name; +}); + #if CONFIG_NAVIGATOR_FULL_MISSION_CACHE_SIZE > 0 class MissionBaseRouteCacheSyncTest : public NavigatorDatamanTestBase { diff --git a/src/modules/navigator/test/test_mission_feasibility_geofence.cpp b/src/modules/navigator/test/test_mission_feasibility_geofence.cpp new file mode 100644 index 00000000000..5e92bdcf259 --- /dev/null +++ b/src/modules/navigator/test/test_mission_feasibility_geofence.cpp @@ -0,0 +1,435 @@ +/**************************************************************************** + * + * Copyright (c) 2026 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* EVENT + * @skip-file + */ + +#include + +#include "mission_feasibility_checker.h" +#include "support/geofence_test_helpers.h" +#include "support/mission_route_test_helpers.h" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +class MissionFeasibilityGeofenceTest : public navigator_test::GeofenceTestBase +{ +protected: + using navigator_test::GeofenceTestBase::GeofenceTestBase; + + static constexpr float kAltitude = 500.f; + + void SetUp() override + { + param_reset_all(); + _navigator.updateParams(); + ASSERT_TRUE(resetFence(kAltitude - 100.f)); + ASSERT_TRUE(_dataman_client.clearSync(DM_KEY_WAYPOINTS_OFFBOARD_0)); + _home_pub.publish(*_navigator.get_home_position()); + + vehicle_status_s status{}; + status.timestamp = hrt_absolute_time(); + status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING; + *_navigator.get_vstatus() = status; + _status_pub.publish(status); + ASSERT_TRUE(loadFence({})); + } + + void TearDown() override { param_reset_all(); } + + mission_item_s waypoint(const matrix::Vector2f &offset) const + { + return navigator_test::makePositionItemFromOffset(_reference(0), _reference(1), offset(0), offset(1), kAltitude); + } + + static mission_item_s changeSpeed() + { + mission_item_s item{}; + item.nav_cmd = NAV_CMD_DO_CHANGE_SPEED; + item.params[0] = 1.f; + item.params[1] = 5.f; + return item; + } + + FencePoints inclusionSquare() const + { + return polygon(true, {{-1000.f, -1000.f}, {1000.f, -1000.f}, {1000.f, 1000.f}, {-1000.f, 1000.f}}); + } + + void drainReports() + { + mavlink_log_s report{}; + event_s event{}; + + while (_log_sub.update(&report)) {} + + while (_event_sub.update(&event)) {} + } + + bool missionFeasible(const std::vector &items) + { + drainReports(); + + for (size_t i = 0; i < items.size(); ++i) { + mission_item_s item = items[i]; + EXPECT_TRUE(_dataman_client.writeSync(DM_KEY_WAYPOINTS_OFFBOARD_0, static_cast(i), + reinterpret_cast(&item), sizeof(item))); + } + + mission_s mission{}; + mission.mission_dataman_id = DM_KEY_WAYPOINTS_OFFBOARD_0; + mission.count = static_cast(items.size()); + MissionFeasibilityChecker checker(&_navigator, _dataman_client); + return checker.checkMissionFeasible(mission); + } + + bool logContains(const char *text) + { + mavlink_log_s report{}; + bool found = false; + + while (_log_sub.update(&report)) { + found |= strstr(reinterpret_cast(report.text), text) != nullptr; + } + + return found; + } + + ::testing::AssertionResult waypointEvent(uint32_t id, int16_t expected_waypoint) + { + event_s event{}; + + while (_event_sub.update(&event)) { + if (event.id == id) { + int16_t waypoint_index; + memcpy(&waypoint_index, event.arguments, sizeof(waypoint_index)); + + if (waypoint_index != expected_waypoint) { + return ::testing::AssertionFailure() << "reported waypoint " << waypoint_index + << ", expected " << expected_waypoint; + } + + return ::testing::AssertionSuccess(); + } + } + + return ::testing::AssertionFailure() << "expected waypoint event not published"; + } + + void expectPathViolation(int16_t waypoint_index) + { + const std::string message = "Geofence breach on path to waypoint " + std::to_string(waypoint_index); + EXPECT_TRUE(logContains(message.c_str())); + EXPECT_TRUE(waypointEvent(events::ID("navigator_mis_geofence_path_violation"), waypoint_index)); + } + + uORB::Publication _home_pub{ORB_ID(home_position)}; + uORB::Publication _status_pub{ORB_ID(vehicle_status)}; + uORB::Subscription _log_sub{ORB_ID(mavlink_log)}; + uORB::Subscription _event_sub{ORB_ID(event)}; +}; + +struct MissionLegCase { + const char *name; + matrix::Vector2f start; + matrix::Vector2f end; + bool feasible; +}; + +class MissionGeofenceLegTest : public MissionFeasibilityGeofenceTest, + public ::testing::WithParamInterface {}; + +TEST_P(MissionGeofenceLegTest, ChecksLegAcrossCommandItems) +{ + const MissionLegCase &test = GetParam(); + ASSERT_TRUE(loadFence(exclusionSquare())); + EXPECT_EQ(missionFeasible({changeSpeed(), waypoint(test.start), changeSpeed(), waypoint(test.end)}), test.feasible); + + if (!test.feasible) { + expectPathViolation(4); + } +} + +INSTANTIATE_TEST_SUITE_P(MissionLegs, MissionGeofenceLegTest, ::testing::Values( + MissionLegCase{"AcrossExclusionPolygon", {0.f, 100.f}, {0.f, 500.f}, false}, + MissionLegCase{"ClearOfExclusionPolygon", {60.f, 100.f}, {60.f, 500.f}, true}), + [](const ::testing::TestParamInfo &test_info) +{ + return test_info.param.name; +}); + +TEST_F(MissionFeasibilityGeofenceTest, FirstPositionMustPassEveryInclusion) +{ + FencePoints points = inclusionSquare(); + const FencePoints smaller = polygon(true, {{-100.f, -100.f}, {100.f, -100.f}, {100.f, 100.f}, {-100.f, 100.f}}); + points.insert(points.end(), smaller.begin(), smaller.end()); + ASSERT_TRUE(loadFence(points)); + EXPECT_FALSE(missionFeasible({changeSpeed(), waypoint({0.f, 200.f})})); + EXPECT_TRUE(waypointEvent(events::ID("navigator_mis_geofence_violation"), 2)); + EXPECT_TRUE(missionFeasible({waypoint({0.f, 0.f}), waypoint({50.f, 50.f})})); +} + +TEST_F(MissionFeasibilityGeofenceTest, SinglePositionOnBoundaryIsRejected) +{ + const FencePoints points = exclusionSquare(); + ASSERT_TRUE(loadFence(points)); + mission_item_s item = waypoint({0.f, 0.f}); + item.lat = points.front().lat; + item.lon = points.front().lon; + ASSERT_TRUE(_fence.checkPointAgainstAllGeofences(item.lat, item.lon, item.altitude)); + EXPECT_FALSE(missionFeasible({item})); + expectPathViolation(1); +} + +TEST_F(MissionFeasibilityGeofenceTest, EmptyFenceAcceptsPositionsAndCommandOnlyMission) +{ + EXPECT_TRUE(missionFeasible({waypoint({0.f, 100.f}), waypoint({0.f, 5000.f})})); + EXPECT_TRUE(missionFeasible({changeSpeed()})); +} + +// Home sits next to the antimeridian. +class AntimeridianMissionGeofenceTest : public MissionFeasibilityGeofenceTest +{ +protected: + AntimeridianMissionGeofenceTest() : MissionFeasibilityGeofenceTest(47.0, 179.999) {} +}; + +TEST_F(AntimeridianMissionGeofenceTest, ShortLegMayCrossTheAntimeridian) +{ + // About 150 m across the antimeridian. + const mission_item_s west = navigator_test::makePositionItem(47.0, 179.999, kAltitude); + const mission_item_s east = navigator_test::makePositionItem(47.0, -179.999, kAltitude); + EXPECT_TRUE(missionFeasible({west, east})); + EXPECT_FALSE(logContains("geofence path check unavailable")); + // An exclusion zone between the waypoint and the antimeridian still blocks the leg. + ASSERT_TRUE(loadFence(polygon(false, {{-20.f, 20.f}, {20.f, 20.f}, {20.f, 60.f}, {-20.f, 60.f}}))); + EXPECT_FALSE(missionFeasible({west, east})); + expectPathViolation(2); +} + +struct MissionBatchCase { + const char *name; + size_t safe_positions; + bool final_leg_breaches; +}; + +class MissionGeofenceBatchTest : public MissionFeasibilityGeofenceTest, + public ::testing::WithParamInterface {}; + +TEST_P(MissionGeofenceBatchTest, ChecksBatchesAndResetsForNextMission) +{ + const MissionBatchCase &test = GetParam(); + ASSERT_TRUE(loadFence(exclusionSquare())); + std::vector items{changeSpeed()}; + + for (size_t i = 0; i < test.safe_positions; ++i) { + items.push_back(waypoint({0.f, 100.f})); + items.push_back(changeSpeed()); + } + + items.push_back(waypoint({0.f, test.final_leg_breaches ? 500.f : 150.f})); + EXPECT_EQ(missionFeasible(items), !test.final_leg_breaches); + + if (test.final_leg_breaches) { + expectPathViolation(static_cast(items.size())); + } + + // The next upload must not retain paths from the previous validation. + EXPECT_TRUE(missionFeasible({waypoint({0.f, 150.f})})); +} + +INSTANTIATE_TEST_SUITE_P(MissionBatches, MissionGeofenceBatchTest, ::testing::Values( + MissionBatchCase{"BreachAtFirstBatchEnd", Geofence::MAX_PATH_CHECKS - 1, true}, + MissionBatchCase{"BreachAfterFirstBatch", Geofence::MAX_PATH_CHECKS, true}, + MissionBatchCase{"ClearFullBatches", 2 * Geofence::MAX_PATH_CHECKS - 1, false}, + MissionBatchCase{"ClearFinalRemainder", 2 * Geofence::MAX_PATH_CHECKS + 1, false}), + [](const ::testing::TestParamInfo &test_info) +{ + return test_info.param.name; +}); + +enum class MissionScalarLimit { Horizontal, Vertical, AltitudeBand }; + +struct MissionScalarCase { + const char *name; + MissionScalarLimit limit; + float east; + float altitude; + bool relative; + bool feasible; +}; + +class MissionGeofenceScalarTest : public MissionFeasibilityGeofenceTest, + public ::testing::WithParamInterface {}; + +TEST_P(MissionGeofenceScalarTest, ChecksEveryPosition) +{ + const MissionScalarCase &test = GetParam(); + ASSERT_TRUE(loadFence(inclusionSquare())); + + if (test.limit == MissionScalarLimit::AltitudeBand) { + GeofenceTestPeer::setAltitudeBand(_fence, 450.f, 550.f); + + } else { + const float maximum = test.limit == MissionScalarLimit::Horizontal ? 300.f : 150.f; + ASSERT_EQ(param_set(param_find(test.limit == MissionScalarLimit::Horizontal ? "GF_MAX_HOR_DIST" : "GF_MAX_VER_DIST"), + &maximum), PX4_OK); + _navigator.updateParams(); + } + + mission_item_s second = waypoint({0.f, test.east}); + second.altitude = test.altitude; + second.altitude_is_relative = test.relative; + EXPECT_EQ(missionFeasible({waypoint({0.f, 100.f}), changeSpeed(), second}), test.feasible); + + if (!test.feasible) { + EXPECT_TRUE(waypointEvent(events::ID("navigator_mis_geofence_violation"), 3)); + } +} + +INSTANTIATE_TEST_SUITE_P(MissionScalarLimits, MissionGeofenceScalarTest, ::testing::Values( + MissionScalarCase{"WithinHomeDistance", MissionScalarLimit::Horizontal, 200.f, 500.f, false, true}, + MissionScalarCase{"BeyondHomeDistance", MissionScalarLimit::Horizontal, 400.f, 500.f, false, false}, + MissionScalarCase{"WithinRelativeAltitude", MissionScalarLimit::Vertical, 200.f, 100.f, true, true}, + MissionScalarCase{"BeyondRelativeAltitude", MissionScalarLimit::Vertical, 200.f, 200.f, true, false}, + MissionScalarCase{"BelowAltitudeBand", MissionScalarLimit::AltitudeBand, 200.f, 440.f, false, false}, + MissionScalarCase{"AboveAltitudeBand", MissionScalarLimit::AltitudeBand, 200.f, 560.f, false, false}, + MissionScalarCase{"AtAltitudeBandMinimum", MissionScalarLimit::AltitudeBand, 200.f, 450.f, false, true}, + MissionScalarCase{"AtAltitudeBandMaximum", MissionScalarLimit::AltitudeBand, 200.f, 550.f, false, true}), + [](const ::testing::TestParamInfo &test_info) +{ + return test_info.param.name; +}); + +TEST_F(MissionFeasibilityGeofenceTest, EmptyFenceStillChecksHomeLimits) +{ + const float maximum = 300.f; + ASSERT_EQ(param_set(param_find("GF_MAX_HOR_DIST"), &maximum), PX4_OK); + _navigator.updateParams(); + EXPECT_FALSE(missionFeasible({waypoint({0.f, 100.f}), waypoint({0.f, 400.f})})); + EXPECT_TRUE(waypointEvent(events::ID("navigator_mis_geofence_violation"), 2)); +} + +TEST_F(MissionFeasibilityGeofenceTest, EmptyFenceIgnoresStoredAltitudeBand) +{ + GeofenceTestPeer::setAltitudeBand(_fence, 450.f, 550.f); + mission_item_s second = waypoint({0.f, 200.f}); + second.altitude = 600.f; + EXPECT_TRUE(missionFeasible({waypoint({0.f, 100.f}), second})); +} + +enum class LaterMissionFailure { Altitude, NoHome }; + +class MissionGeofenceFailureOrderTest : public MissionFeasibilityGeofenceTest, + public ::testing::WithParamInterface {}; + +TEST_P(MissionGeofenceFailureOrderTest, EarlierPathBreachPrecedesLaterFailure) +{ + ASSERT_TRUE(loadFence(exclusionSquare())); + mission_item_s invalid = waypoint({100.f, 500.f}); + + if (GetParam() == LaterMissionFailure::NoHome) { + _navigator.get_home_position()->valid_hpos = false; + _home_pub.publish(*_navigator.get_home_position()); + invalid.altitude_is_relative = true; + invalid.altitude = 100.f; + + } else { + const float maximum = 150.f; + ASSERT_EQ(param_set(param_find("GF_MAX_VER_DIST"), &maximum), PX4_OK); + _navigator.updateParams(); + invalid.altitude = 600.f; + } + + // The first leg crosses the exclusion, but is still buffered when the third item fails. + EXPECT_FALSE(missionFeasible({waypoint({0.f, 100.f}), waypoint({0.f, 500.f}), invalid})); + expectPathViolation(2); +} + +INSTANTIATE_TEST_SUITE_P(MissionFailureOrder, MissionGeofenceFailureOrderTest, + ::testing::Values(LaterMissionFailure::Altitude, LaterMissionFailure::NoHome), + [](const ::testing::TestParamInfo &test_info) +{ + return test_info.param == LaterMissionFailure::NoHome ? "MissingHome" : "AltitudeLimit"; +}); + +TEST_F(MissionFeasibilityGeofenceTest, RelativeAltitudeRequiresValidHome) +{ + _navigator.get_home_position()->valid_hpos = false; + _home_pub.publish(*_navigator.get_home_position()); + mission_item_s relative = waypoint({0.f, 100.f}); + relative.altitude_is_relative = true; + relative.altitude = 100.f; + EXPECT_FALSE(missionFeasible({relative})); + EXPECT_TRUE(logContains("Geofence requires valid home position")); +} + +TEST_F(MissionFeasibilityGeofenceTest, MissingFenceCacheRejectsUpload) +{ + ASSERT_TRUE(loadFence(exclusionSquare())); + GeofenceTestPeer::invalidateCache(_fence); + EXPECT_FALSE(missionFeasible({waypoint({100.f, 100.f}), waypoint({100.f, 500.f})})); + EXPECT_TRUE(logContains("geofence path check unavailable")); +} + +TEST_F(MissionFeasibilityGeofenceTest, PendingFenceRefreshRejectsUpload) +{ + _fence.updateFence(); + EXPECT_FALSE(missionFeasible({waypoint({0.f, 100.f})})); + EXPECT_TRUE(logContains("geofence path check unavailable")); +} + +TEST_F(MissionFeasibilityGeofenceTest, UnreadableMissionStorageRejectsUpload) +{ + drainReports(); + mission_s mission{}; + // This key cannot hold a mission_item_s, so readSync fails without a timeout. + mission.mission_dataman_id = DM_KEY_FENCE_POINTS_STATE; + mission.count = 1; + MissionFeasibilityChecker checker(&_navigator, _dataman_client); + EXPECT_FALSE(checker.checkMissionFeasible(mission)); + EXPECT_TRUE(logContains("dataman read failed at item 0")); + EXPECT_TRUE(waypointEvent(events::ID("navigator_mis_dm_read_fail"), 0)); +} diff --git a/src/modules/navigator/test/test_prec_takeoff_integration.cpp b/src/modules/navigator/test/test_prec_takeoff_integration.cpp index 9bd362e22cb..8c809cf3b5d 100644 --- a/src/modules/navigator/test/test_prec_takeoff_integration.cpp +++ b/src/modules/navigator/test/test_prec_takeoff_integration.cpp @@ -53,6 +53,7 @@ #include #include #include +#include #include #include #include @@ -303,16 +304,28 @@ TEST_F(PrecTakeoffIntegrationTest, MissionTakeoffCorrectionPreservesPlannedDesti uORB::Publication vehicle_status_pub{ORB_ID(vehicle_status)}; uORB::Publication global_pos_pub{ORB_ID(vehicle_global_position)}; uORB::Publication land_detected_pub{ORB_ID(vehicle_land_detected)}; - uORB::Publication geofence_status_pub{ORB_ID(geofence_status)}; uORB::Publication mission_pub{ORB_ID(mission)}; ASSERT_TRUE(home_pub.publish(home)); ASSERT_TRUE(vehicle_status_pub.publish(vehicle_status)); ASSERT_TRUE(global_pos_pub.publish(*_navigator->get_global_position())); ASSERT_TRUE(land_detected_pub.publish(*_navigator->get_land_detected())); - geofence_status_s geofence_status{}; - geofence_status.timestamp = hrt_absolute_time(); - geofence_status.status = geofence_status_s::GF_STATUS_READY; - ASSERT_TRUE(geofence_status_pub.publish(geofence_status)); + + // Load an empty fence so mission validation sees the completed fence update. + DatamanClient dataman; + mission_stats_entry_s fence_stats{}; + fence_stats.dataman_id = DM_KEY_FENCE_POINTS_0; + ASSERT_TRUE(dataman.writeSync(DM_KEY_FENCE_POINTS_STATE, 0, + reinterpret_cast(&fence_stats), sizeof(fence_stats))); + Geofence &fence = _navigator->get_geofence(); + fence.updateFence(); + const hrt_abstime start = hrt_absolute_time(); + + while (!fence.isReadyForPathChecks() && hrt_elapsed_time(&start) < 1_s) { + fence.run(); + px4_usleep(1000); + } + + ASSERT_TRUE(fence.isReadyForPathChecks()); mission_item_s planned_takeoff{}; planned_takeoff.nav_cmd = NAV_CMD_VTOL_TAKEOFF; @@ -326,7 +339,6 @@ TEST_F(PrecTakeoffIntegrationTest, MissionTakeoffCorrectionPreservesPlannedDesti planned_land.nav_cmd = NAV_CMD_VTOL_LAND; planned_land.altitude = kGroundAltitude; - DatamanClient dataman; ASSERT_TRUE(dataman.clearSync(DM_KEY_WAYPOINTS_OFFBOARD_0)); ASSERT_TRUE(dataman.writeSync(DM_KEY_WAYPOINTS_OFFBOARD_0, 0, reinterpret_cast(&planned_takeoff), sizeof(planned_takeoff)));