added unit tests

Signed-off-by: RomanBapst <bapstroman@gmail.com>

src/modules/navigator/MissionFeasibility/CMakeLists.txt

@@ -2,4 +2,6 @@ px4_add_library(mission_feasibility_checker
 	FeasibilityChecker.cpp
 )
 
-px4_add_functional_gtest(SRC FeasibilityCheckerTest.cpp LINKLIBS mission_feasibility_checker test_stubs)
+target_link_libraries(mission_feasibility_checker PUBLIC modules__navigator modules__dataman)
+
+px4_add_functional_gtest(SRC FeasibilityCheckerTest.cpp LINKLIBS mission_feasibility_checker)

src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp

@@ -46,6 +46,13 @@ FeasibilityChecker::FeasibilityChecker() :
 
 void FeasibilityChecker::reset()
 {
+
+	_is_landed = false;
+	_home_alt_msl = NAN;
+	_home_lat_lon = matrix::Vector2d(NAN, NAN);
+	_vehicle_type = 0;
+	_is_vtol = false;
+
 	_mission_validity_failed = false;
 	_takeoff_failed = false;
 	_fixed_wing_landing_failed = false;
@@ -75,7 +82,8 @@ void FeasibilityChecker::updateData()
 {
 	home_position_s home = {};
 
-	if (_home_pos_sub.copy(&home)) {
+	if (_home_pos_sub.updated()) {
+		_home_pos_sub.update(&home);
 		if (home.valid_hpos) {
 			_home_lat_lon = matrix::Vector2d(home.lat, home.lon);
 		}
@@ -87,14 +95,16 @@ void FeasibilityChecker::updateData()
 
 	vehicle_status_s status = {};
 
-	if (_status_sub.copy(&status)) {
+	if (_status_sub.updated()) {
+		_status_sub.copy(&status);
 		_vehicle_type = status.vehicle_type;
 		_is_vtol = status.is_vtol;
 	}
 
 	vehicle_land_detected_s land_detected = {};
 
-	if (_land_detector_sub.copy(&land_detected)) {
+	if (_land_detector_sub.updated()) {
+		_land_detector_sub.copy(&land_detected);
 		_is_landed = land_detected.landed;
 	}
 
@@ -119,7 +129,7 @@ void FeasibilityChecker::processNextItem(mission_item_s &mission_item, const int
 		doMulticopterChecks(mission_item, current_index);
 	}
 
-	if (current_index == total_count) {
+	if (current_index == total_count - 1) {
 		_takeoff_land_available_failed = !checkTakeoffLandAvailable();
 	}
 
@@ -162,7 +172,7 @@ void FeasibilityChecker::doFixedWingChecks(mission_item_s &mission_item, const i
 		_fixed_wing_landing_failed = !checkFixedWingLanding(mission_item, current_index);
 	}
 
-	if (_fixed_wing_land_approach_failed) {
+	if (!_fixed_wing_land_approach_failed) {
 		_fixed_wing_land_approach_failed = !checkFixedWindLandApproach(mission_item, current_index);
 	}
 
@@ -431,12 +441,6 @@ bool FeasibilityChecker::checkFixedWindLandApproach(mission_item_s &mission_item
 
 			_landing_valid = true;
 
-		} else {
-			// mission item before land doesn't have a position
-			mavlink_log_critical(_mavlink_log_pub, "Mission rejected: need landing approach.\t");
-			events::send(events::ID("navigator_mis_req_landing_approach"), {events::Log::Error, events::LogInternal::Info},
-				     "Mission rejected: landing approach is required");
-			return false;
 		}
 	}
 
@@ -445,7 +449,6 @@ bool FeasibilityChecker::checkFixedWindLandApproach(mission_item_s &mission_item
 
 bool FeasibilityChecker::checkFixedWingLanding(mission_item_s &mission_item, const int current_index)
 {
-	const bool land_start_found = _do_land_start_index > 0;
 
 	// if DO_LAND_START found then require valid landing AFTER
 	if (mission_item.nav_cmd == NAV_CMD_DO_LAND_START) {
@@ -460,6 +463,8 @@ bool FeasibilityChecker::checkFixedWingLanding(mission_item_s &mission_item, con
 		_do_land_start_index = current_index;
 	}
 
+	const bool land_start_found = _do_land_start_index >= 0;
+
 	if (mission_item.nav_cmd == NAV_CMD_LAND || mission_item.nav_cmd == NAV_CMD_VTOL_LAND) {
 
 		if (current_index > 0) {
@@ -563,7 +568,7 @@ bool FeasibilityChecker::checkTakeoffLandAvailable()
 
 bool FeasibilityChecker::checkDistanceToFirstWaypoint(mission_item_s &mission_item)
 {
-	if (_param_mis_dist_1wp.get() <= 0.0f) {
+	if (_param_mis_dist_1wp.get() <= 0.0f || !_home_lat_lon.isAllFinite()) {
 		/* param not set, check is ok */
 		return true;
 	}

src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp

@@ -60,6 +60,7 @@ public:
 		       _distance_first_waypoint_failed ||
 		       _distance_between_waypoints_failed ||
 		       _fixed_wing_landing_failed ||
+		       _fixed_wing_land_approach_failed ||
 		       _below_home_alt_failed ||
 		       _mission_validity_failed ||
 		       _takeoff_land_available_failed;

src/modules/navigator/MissionFeasibility/FeasibilityCheckerTest.cpp

@@ -33,9 +33,12 @@
 
 #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>
+
 
 // to run: make tests TESTFILTER=FeasibilityChecker
-
 class FeasibilityCheckerTest : public ::testing::Test
 {
 public:
@@ -51,7 +54,410 @@ class TestFeasibilityChecker : public FeasibilityChecker
 public:
 	TestFeasibilityChecker() : FeasibilityChecker() {}
 	void paramsChanged() {FeasibilityChecker::updateParamsImpl();}
+
+	void publishHomePosition(double lat, double lon, float alt)
+	{
+		home_position_s home = {};
+		home.alt = alt;
+		home.valid_alt = true;
+		home.lat = lat;
+		home.lon = lon;
+		home.valid_hpos = true;
+		orb_advert_t home_pub = orb_advertise(ORB_ID(home_position), &home);
+		orb_publish(ORB_ID(home_position), home_pub, &home);
+	}
+
+	void publishLanded(bool landed)
+	{
+		vehicle_land_detected_s land_detected = {};
+		land_detected.landed = true;
+		orb_advert_t landed_pub = orb_advertise(ORB_ID(vehicle_land_detected), &land_detected);
+		orb_publish(ORB_ID(vehicle_land_detected), landed_pub, &land_detected);
+	}
+
+	void publishVehicleType(int vehicle_type)
+	{
+		vehicle_status_s status = {};
+		status.vehicle_type = vehicle_type;
+		orb_advert_t status_pub = orb_advertise(ORB_ID(vehicle_status), &status);
+		orb_publish(ORB_ID(vehicle_status), status_pub, &status);
+	}
+
 };
 
+TEST_F(FeasibilityCheckerTest, instantiation)
+{
+	FeasibilityChecker checker();
+}
 
-TEST_F(FeasibilityCheckerTest, instantiation) { FeasibilityChecker cp(); }
+TEST_F(FeasibilityCheckerTest, mission_item_validity)
+{
+	TestFeasibilityChecker checker;
+
+
+	checker.publishLanded(true);
+
+	// supported mission item should pass
+	mission_item_s mission_item = {};
+	mission_item.nav_cmd = NAV_CMD_WAYPOINT;
+	checker.processNextItem(mission_item, 0, 1);
+	ASSERT_EQ(checker.someCheckFailed(), false);
+
+	// when landed the first item cannot be a land item
+	checker.reset();
+	checker.publishLanded(true);
+	mission_item.nav_cmd = NAV_CMD_LAND;
+	checker.processNextItem(mission_item, 0, 1);
+	ASSERT_EQ(checker.someCheckFailed(), true);
+}
+
+TEST_F(FeasibilityCheckerTest, max_dist_between_waypoints)
+{
+	TestFeasibilityChecker checker;
+	checker.publishLanded(true);
+
+	param_t param = param_handle(px4::params::MIS_DIST_WPS);
+
+	float max_dist = 1000.0f;
+	param_set(param, &max_dist);
+	checker.paramsChanged();
+
+	mission_item_s mission_item = {};
+	mission_item.nav_cmd = NAV_CMD_WAYPOINT;
+
+	checker.processNextItem(mission_item, 0, 3);
+
+	// waypoints are within limits, check should pass
+	double lat_new, lon_new;
+	waypoint_from_heading_and_distance(mission_item.lat, mission_item.lon, 0, 999, &lat_new, &lon_new);
+	mission_item.lat = lat_new;
+	mission_item.lon = lon_new;
+	checker.processNextItem(mission_item, 1, 3);
+
+	ASSERT_EQ(checker.someCheckFailed(), false);
+
+	// waypoints are above limits, check should fail
+	waypoint_from_heading_and_distance(mission_item.lat, mission_item.lon, 0, 1001, &lat_new, &lon_new);
+
+	mission_item.lat = lat_new;
+	mission_item.lon = lon_new;
+	checker.processNextItem(mission_item, 2, 3);
+
+	ASSERT_EQ(checker.someCheckFailed(), true);
+}
+
+
+TEST_F(FeasibilityCheckerTest, check_dist_first_waypoint)
+{
+	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;
+	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);
+
+	ASSERT_EQ(checker.someCheckFailed(), true);
+
+	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);
+
+	ASSERT_EQ(checker.someCheckFailed(), false);
+}
+
+TEST_F(FeasibilityCheckerTest, check_below_home)
+{
+	TestFeasibilityChecker checker;
+	checker.publishLanded(true);
+	//checker.publishHomePosition(0,0,100);
+
+
+	mission_item_s mission_item = {};
+	mission_item.nav_cmd = NAV_CMD_WAYPOINT;
+	mission_item.altitude = 50;
+	mission_item.altitude_is_relative = true;
+
+	checker.processNextItem(mission_item, 0, 1);
+
+	// this is done to invalidate the home position
+	checker.reset();
+	checker.publishLanded(true);
+	checker.processNextItem(mission_item, 0, 1);
+
+	// cannot have relative altitude without valid home position
+	ASSERT_EQ(checker.someCheckFailed(), true);
+}
+
+TEST_F(FeasibilityCheckerTest, check_takeoff)
+{
+	TestFeasibilityChecker checker;
+	checker.publishLanded(true);
+	checker.publishHomePosition(0, 0, 100);
+
+
+	// takeoff altitude is smaller than acceptance radius, should fail
+	mission_item_s mission_item = {};
+	mission_item.nav_cmd = NAV_CMD_TAKEOFF;
+	mission_item.altitude = 5.0f;
+	mission_item.altitude_is_relative = true;
+	mission_item.acceptance_radius = 10.0f;
+
+	checker.processNextItem(mission_item, 0, 1);
+
+	ASSERT_EQ(checker.someCheckFailed(), true);
+
+	checker.reset();
+	checker.publishLanded(true);
+	checker.publishHomePosition(0, 0, 100);
+
+
+	// takeoff altitude is larger than acceptance radius, should pass
+	mission_item.acceptance_radius = 3.0f;
+	checker.processNextItem(mission_item, 0, 1);
+
+	ASSERT_EQ(checker.someCheckFailed(), false);
+
+
+	// takeoff item needs to be first item
+	checker.reset();
+	checker.publishLanded(true);
+	checker.publishHomePosition(0, 0, 100);
+
+	mission_item.nav_cmd = NAV_CMD_WAYPOINT;
+
+	checker.processNextItem(mission_item, 0, 2);
+
+	mission_item.nav_cmd = NAV_CMD_TAKEOFF;
+
+	checker.processNextItem(mission_item, 1, 2);
+
+	ASSERT_EQ(checker.someCheckFailed(), true);
+}
+
+TEST_F(FeasibilityCheckerTest, fixed_wing_land_approach)
+{
+	TestFeasibilityChecker checker;
+	checker.publishLanded(true);
+	checker.publishHomePosition(0, 0, 100);
+	checker.publishVehicleType(vehicle_status_s::VEHICLE_TYPE_FIXED_WING);
+
+	mission_item_s mission_item = {};
+	mission_item.nav_cmd = NAV_CMD_WAYPOINT;
+	mission_item.altitude = 50;
+	mission_item.altitude_is_relative = true;
+
+	checker.processNextItem(mission_item, 0, 2);
+
+	mission_item.nav_cmd = NAV_CMD_LAND;
+	mission_item.altitude = 60;
+	checker.processNextItem(mission_item, 1, 2);
+
+	ASSERT_EQ(checker.someCheckFailed(), true);
+
+
+	// landing point should not be within loiter radius of previous waypoint
+	checker.reset();
+	checker.publishLanded(true);
+	checker.publishVehicleType(vehicle_status_s::VEHICLE_TYPE_FIXED_WING);
+	checker.publishHomePosition(0, 0, 100);
+	mission_item.nav_cmd = NAV_CMD_LOITER_TO_ALT;
+	mission_item.altitude = 50;
+	mission_item.loiter_radius = 100;
+
+	checker.processNextItem(mission_item, 0, 2);
+
+	double lat_new, lon_new;
+	waypoint_from_heading_and_distance(mission_item.lat, mission_item.lon, 0, 99, &lat_new, &lon_new);
+	mission_item.lat = lat_new;
+	mission_item.lon = lon_new;
+	mission_item.nav_cmd = NAV_CMD_LAND;
+	mission_item.altitude = 40;
+
+	checker.processNextItem(mission_item, 1, 2);
+
+	ASSERT_EQ(checker.someCheckFailed(), true);
+
+	// only loiter to alt or plain waypoint type are allowed before landing
+	checker.reset();
+	checker.publishLanded(true);
+	checker.publishVehicleType(vehicle_status_s::VEHICLE_TYPE_FIXED_WING);
+	checker.publishHomePosition(0, 0, 100);
+	mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
+	mission_item.altitude = 50;
+	mission_item.loiter_radius = 50;
+
+	checker.processNextItem(mission_item, 0, 2);
+
+	waypoint_from_heading_and_distance(mission_item.lat, mission_item.lon, 0, 99, &lat_new, &lon_new);
+	mission_item.lat = lat_new;
+	mission_item.lon = lon_new;
+	mission_item.nav_cmd = NAV_CMD_LAND;
+	mission_item.altitude = 40;
+
+	checker.processNextItem(mission_item, 1, 2);
+
+	ASSERT_EQ(checker.someCheckFailed(), true);
+
+	// fail the glide slope
+	checker.reset();
+	checker.publishLanded(true);
+	checker.publishVehicleType(vehicle_status_s::VEHICLE_TYPE_FIXED_WING);
+	checker.publishHomePosition(0, 0, 100);
+	mission_item.nav_cmd = NAV_CMD_WAYPOINT;
+	mission_item.altitude = 50;
+	mission_item.loiter_radius = 50;
+
+	checker.processNextItem(mission_item, 0, 2);
+
+	waypoint_from_heading_and_distance(mission_item.lat, mission_item.lon, 0, 1, &lat_new, &lon_new);
+	mission_item.lat = lat_new;
+	mission_item.lon = lon_new;
+	mission_item.nav_cmd = NAV_CMD_LAND;
+	mission_item.altitude = 40;
+
+	checker.processNextItem(mission_item, 1, 2);
+
+	ASSERT_EQ(checker.someCheckFailed(), true);
+
+	// fixed wing land approach checks should not be executed for rotary wing
+	checker.publishVehicleType(vehicle_status_s::VEHICLE_TYPE_ROTARY_WING);
+	checker.reset();
+	checker.publishLanded(true);
+	checker.publishHomePosition(0, 0, 100);
+	mission_item.nav_cmd = NAV_CMD_WAYPOINT;
+	mission_item.altitude = 50;
+	mission_item.loiter_radius = 50;
+
+	checker.processNextItem(mission_item, 0, 2);
+
+	waypoint_from_heading_and_distance(mission_item.lat, mission_item.lon, 0, 1, &lat_new, &lon_new);
+	mission_item.lat = lat_new;
+	mission_item.lon = lon_new;
+	mission_item.nav_cmd = NAV_CMD_LAND;
+	mission_item.altitude = 40;
+
+	checker.processNextItem(mission_item, 1, 2);
+
+	ASSERT_EQ(checker.someCheckFailed(), false);
+}
+
+TEST_F(FeasibilityCheckerTest, fixed_wing_landing)
+{
+	// multiple do land start are not allowed
+	TestFeasibilityChecker checker;
+	checker.publishLanded(true);
+	checker.publishHomePosition(0, 0, 100);
+	checker.publishVehicleType(vehicle_status_s::VEHICLE_TYPE_FIXED_WING);
+
+	mission_item_s mission_item = {};
+	mission_item.nav_cmd = NAV_CMD_DO_LAND_START;
+
+	checker.processNextItem(mission_item, 0, 2);
+	checker.processNextItem(mission_item, 1, 2);
+
+	ASSERT_EQ(checker.someCheckFailed(), true);
+
+
+	// cannot start with land waypoint
+	checker.reset();
+	checker.publishVehicleType(vehicle_status_s::VEHICLE_TYPE_FIXED_WING);
+	mission_item.nav_cmd = NAV_CMD_LAND;
+	checker.processNextItem(mission_item, 0, 2);
+	ASSERT_EQ(checker.someCheckFailed(), true);
+
+	// cannot have land start before RTL
+	checker.reset();
+	checker.publishVehicleType(vehicle_status_s::VEHICLE_TYPE_FIXED_WING);
+	mission_item.nav_cmd = NAV_CMD_DO_LAND_START;
+	checker.processNextItem(mission_item, 0, 2);
+
+	mission_item.nav_cmd = NAV_CMD_RETURN_TO_LAUNCH;
+	checker.processNextItem(mission_item, 1, 2);
+	ASSERT_EQ(checker.someCheckFailed(), true);
+
+	// cannot have land start after landing waypoint
+	checker.reset();
+	checker.publishHomePosition(0, 0, 100);
+	checker.publishVehicleType(vehicle_status_s::VEHICLE_TYPE_FIXED_WING);
+	mission_item.nav_cmd = NAV_CMD_WAYPOINT;
+	mission_item.altitude = 10;
+	mission_item.altitude_is_relative = true;
+	checker.processNextItem(mission_item, 0, 3);
+
+	mission_item.nav_cmd = NAV_CMD_LAND;
+	mission_item.altitude = 0;
+	double lat_new, lon_new;
+	waypoint_from_heading_and_distance(mission_item.lat, mission_item.lon, 0, 200, &lat_new, &lon_new);
+	mission_item.lat = lat_new;
+	mission_item.lon = lon_new;
+	checker.processNextItem(mission_item, 1, 3);
+
+	mission_item.nav_cmd = NAV_CMD_DO_LAND_START;
+	checker.processNextItem(mission_item, 2, 3);
+	ASSERT_EQ(checker.someCheckFailed(), true);
+}
+
+TEST_F(FeasibilityCheckerTest, check_takeoff_land_requirements)
+{
+
+	TestFeasibilityChecker checker;
+	checker.publishLanded(true);
+	checker.publishHomePosition(0, 0, 0);
+
+	param_t param = param_handle(px4::params::MIS_TKO_LAND_REQ);
+
+	// not takeoff land requiremntes, check should pass
+	int param_val = 0;
+	param_set(param, &param_val);
+	checker.paramsChanged();
+
+	mission_item_s mission_item = {};
+	mission_item.nav_cmd = NAV_CMD_WAYPOINT;
+
+	checker.processNextItem(mission_item, 0, 1);
+	ASSERT_EQ(checker.someCheckFailed(), false);
+
+	// require takeoff, check should fail
+	param_val = 1;
+	param_set(param, &param_val);
+	checker.paramsChanged();
+	checker.processNextItem(mission_item, 0, 1);
+	ASSERT_EQ(checker.someCheckFailed(), true);
+
+	// require landing, check should fail
+	param_val = 2;
+	param_set(param, &param_val);
+	checker.paramsChanged();
+	checker.processNextItem(mission_item, 0, 1);
+	ASSERT_EQ(checker.someCheckFailed(), true);
+
+	// require both takeoff and landing, check should fail
+	param_val = 3;
+	param_set(param, &param_val);
+	checker.paramsChanged();
+	checker.processNextItem(mission_item, 0, 1);
+	ASSERT_EQ(checker.someCheckFailed(), true);
+
+	// require takeoff and land or none, this should pass as we don't have either
+	param_val = 4;
+	param_set(param, &param_val);
+	checker.paramsChanged();
+	checker.processNextItem(mission_item, 0, 1);
+	ASSERT_EQ(checker.someCheckFailed(), false);
+}