feat(navigator): check geofence breaches for mission segments (not only waypoints) (#28760)

* feat(navigator): check geofence breaches for mission segments (not only waypoints)

* refactor(navigator): move mission path batch off the stack

* fix(navigator): keep the mission verdict while the fence reloads

* test(navigator): check a mission leg across the antimeridian

---------

Co-authored-by: jonas <jonas.perolini@rigi.tech>
This commit is contained in:
Jonas Peroliniandjonas authored and GitHub committed 2026-10-07 18:06:33 +02:00
1 parent 92b8bc4342
commit dd804e4b9c
14 files changed
+916 -43

No files matched your search

+1
View File
@@ -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
@@ -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
+1
View File
@@ -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
+9
View File
@@ -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
+11 -4
View File
@@ -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 */
+10
View File
@@ -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();
+25 -5
View File
@@ -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);
+1
View File
@@ -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 */
@@ -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<uint8_t *>(&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<uint8_t *>(&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<int16_t>(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<uint16_t>(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<int16_t>(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<unsigned>(waypoint));
events::send<uint16_t>(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)
{
@@ -46,17 +46,32 @@
#include <uORB/topics/mission.h>
#include <px4_platform_common/module_params.h>
#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:
@@ -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)
@@ -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 <vector>
#include <cstring>
#include <px4_platform_common/posix.h>
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/home_position.h>
#include <uORB/topics/mavlink_log.h>
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<uint8_t *>(&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<uint8_t *>(&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<uint8_t *>(&item), sizeof(item));
}
DatamanClient _dataman_client{};
MissionBaseTestNavigator _navigator{};
MissionBaseTestPeer _mission_base{&_navigator};
uORB::Publication<home_position_s> _home_pub{ORB_ID(home_position)};
uORB::Publication<vehicle_status_s> _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<ChangedMissionInput> {};
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<ChangedMissionInput> &test_info)
{
return test_info.param.name;
});
#if CONFIG_NAVIGATOR_FULL_MISSION_CACHE_SIZE > 0
class MissionBaseRouteCacheSyncTest : public NavigatorDatamanTestBase
{
@@ -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 <gtest/gtest.h>
#include "mission_feasibility_checker.h"
#include "support/geofence_test_helpers.h"
#include "support/mission_route_test_helpers.h"
#include <cmath>
#include <cstring>
#include <string>
#include <vector>
#include <px4_platform_common/events.h>
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/event.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/mavlink_log.h>
#include <uORB/topics/vehicle_status.h>
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<mission_item_s> &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<uint32_t>(i),
reinterpret_cast<uint8_t *>(&item), sizeof(item)));
}
mission_s mission{};
mission.mission_dataman_id = DM_KEY_WAYPOINTS_OFFBOARD_0;
mission.count = static_cast<uint16_t>(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<const char *>(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_position_s> _home_pub{ORB_ID(home_position)};
uORB::Publication<vehicle_status_s> _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<MissionLegCase> {};
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<MissionLegCase> &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<MissionBatchCase> {};
TEST_P(MissionGeofenceBatchTest, ChecksBatchesAndResetsForNextMission)
{
const MissionBatchCase &test = GetParam();
ASSERT_TRUE(loadFence(exclusionSquare()));
std::vector<mission_item_s> 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<int16_t>(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<MissionBatchCase> &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<MissionScalarCase> {};
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<MissionScalarCase> &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<LaterMissionFailure> {};
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<LaterMissionFailure> &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));
}
@@ -53,6 +53,7 @@
#include <drivers/drv_hrt.h>
#include <lib/geo/geo.h>
#include <lib/parameters/param.h>
#include <px4_platform_common/posix.h>
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/landing_target_pose.h>
@@ -303,16 +304,28 @@ TEST_F(PrecTakeoffIntegrationTest, MissionTakeoffCorrectionPreservesPlannedDesti
uORB::Publication<vehicle_status_s> vehicle_status_pub{ORB_ID(vehicle_status)};
uORB::Publication<vehicle_global_position_s> global_pos_pub{ORB_ID(vehicle_global_position)};
uORB::Publication<vehicle_land_detected_s> land_detected_pub{ORB_ID(vehicle_land_detected)};
uORB::Publication<geofence_status_s> geofence_status_pub{ORB_ID(geofence_status)};
uORB::Publication<mission_s> 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<uint8_t *>(&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<uint8_t *>(&planned_takeoff), sizeof(planned_takeoff)));