Navigator: take vehicle position for first-WP-distance-check

And only use Home position if current vehicle position is unknown.

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

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()) {
+	matrix::Vector2d position_reference = matrix::Vector2d((double)NAN, (double)NAN);
-		/* param not set, check is ok */
+
-		return true;
+	// 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(
+		const float dist_to_1wp = get_distance_to_next_waypoint(
-					    mission_item.lat, mission_item.lon,
+						  mission_item.lat, mission_item.lon,
-					    _home_lat_lon(0), _home_lat_lon(1));
+						  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;
 }
 

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 <mathlib/mathlib.h>
 #include <uORB/topics/home_position.h>
 #include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_land_detected.h>
 #include <uORB/Subscription.hpp>
 #include <px4_platform_common/module_params.h>
@@ -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

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 <gtest/gtest.h>
 #include "FeasibilityChecker.hpp"
-#include <uORB/topics/home_position.h>
-#include <uORB/topics/vehicle_land_detected.h>
 #include <lib/geo/geo.h>
 
 
@@ -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);
 }