Fix mission check for init mission

msg/CMakeLists.txt

@@ -99,6 +99,7 @@ set(msg_files
 	FollowTargetStatus.msg
 	GeneratorStatus.msg
 	GeofenceResult.msg
+	GeofenceStatus.msg
 	GimbalControls.msg
 	GimbalDeviceAttitudeStatus.msg
 	GimbalDeviceInformation.msg

msg/GeofenceStatus.msg

@@ -0,0 +1,7 @@
+uint64 timestamp                        # time since system start (microseconds)
+
+uint32 geofence_id 			# loaded geofence id
+uint8 status 				# Current geofence status
+
+uint8 GF_STATUS_LOADING = 0
+uint8 GF_STATUS_READY = 1

msg/MissionResult.msg

@@ -1,8 +1,8 @@
 uint64 timestamp		# time since system start (microseconds)
 
-uint16 mission_id   		# Id for the mission for which the result was generated
-uint16 geofence_id  		# Id for the corresponding geofence for which the result was generated (used for mission feasibility)
-uint64 home_position_counter  	# Counter of the home position for which the result was generated (used for mission feasibility)
+uint32 mission_id   		# Id for the mission for which the result was generated
+uint32 geofence_id  		# Id for the corresponding geofence for which the result was generated (used for mission feasibility)
+uint32 home_position_counter  	# Counter of the home position for which the result was generated (used for mission feasibility)
 
 int32 seq_reached		# Sequence of the mission item which has been reached, default -1
 uint16 seq_current		# Sequence of the current mission item

src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp

@@ -46,13 +46,6 @@ FeasibilityChecker::FeasibilityChecker() :
 
 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;
 	_takeoff_failed = false;
 	_land_pattern_validity_failed = false;
@@ -86,10 +79,16 @@ void FeasibilityChecker::updateData()
 
 		if (home.valid_hpos) {
 			_home_lat_lon = matrix::Vector2d(home.lat, home.lon);
+
+		} else {
+			_home_lat_lon = matrix::Vector2d((double)NAN, (double)NAN);
 		}
 
 		if (home.valid_alt) {
 			_home_alt_msl = home.alt;
+
+		} else {
+			_home_alt_msl = NAN;
 		}
 	}
 

src/modules/navigator/MissionFeasibility/FeasibilityCheckerTest.cpp

@@ -65,6 +65,18 @@ class TestFeasibilityChecker : public FeasibilityChecker
 		orb_publish(ORB_ID(home_position), home_pub, &home);
 	}
 
+	void publishInvalidHome()
+	{
+		home_position_s home = {};
+		home.alt = 0.f;
+		home.valid_alt = false;
+		home.lat = 0.;
+		home.lon = 0.;
+		home.valid_hpos = false;
+		orb_advert_t home_pub = orb_advertise(ORB_ID(home_position), &home);
+		orb_publish(ORB_ID(home_position), home_pub, &home);
+	}
+
 	void publishCurrentPosition(double lat, double lon)
 	{
 		vehicle_global_position_s gpos = {};
@@ -122,6 +134,7 @@ TEST_F(FeasibilityCheckerTest, mission_item_validity)
 	ASSERT_EQ(ret, false);
 
 	checker.reset();
+	checker.publishInvalidHome();
 	mission_item.nav_cmd = NAV_CMD_TAKEOFF;
 	mission_item.altitude_is_relative = true;
 	ret = checker.processNextItem(mission_item, 0, 5);
@@ -190,6 +203,7 @@ TEST_F(FeasibilityCheckerTest, check_below_home)
 
 	// this is done to invalidate the home position
 	checker.reset();
+	checker.publishInvalidHome();
 	checker.publishLanded(true);
 	checker.processNextItem(mission_item, 0, 1);
 

src/modules/navigator/geofence.cpp

@@ -81,6 +81,8 @@ Geofence::Geofence(Navigator *navigator) :
 	if (_navigator != nullptr) {
 		updateFence();
 	}
+
+	_geofence_status_pub.advertise();
 }
 
 Geofence::~Geofence()
@@ -101,6 +103,14 @@ void Geofence::run()
 		if (_initiate_fence_updated) {
 			_initiate_fence_updated = false;
 			_dataman_state	= DatamanState::Read;
+
+			geofence_status_s status;
+			status.timestamp = hrt_absolute_time();
+			status.geofence_id = _opaque_id;
+			status.status = geofence_status_s::GF_STATUS_LOADING;
+
+			_geofence_status_pub.publish(status);
+
 		}
 
 		break;
@@ -147,6 +157,14 @@ void Geofence::run()
 
 			} else {
 				_dataman_state = DatamanState::UpdateRequestWait;
+				_fence_updated = true;
+
+				geofence_status_s status;
+				status.timestamp = hrt_absolute_time();
+				status.geofence_id = _opaque_id;
+				status.status = geofence_status_s::GF_STATUS_READY;
+
+				_geofence_status_pub.publish(status);
 			}
 		}
 
@@ -160,6 +178,13 @@ void Geofence::run()
 			_dataman_state = DatamanState::UpdateRequestWait;
 			_updateFence();
 			_fence_updated = true;
+
+			geofence_status_s status;
+			status.timestamp = hrt_absolute_time();
+			status.geofence_id = _opaque_id;
+			status.status = geofence_status_s::GF_STATUS_READY;
+
+			_geofence_status_pub.publish(status);
 		}
 
 		break;

src/modules/navigator/geofence.h

@@ -49,6 +49,7 @@
 #include <lib/geo/geo.h>
 #include <px4_platform_common/defines.h>
 #include <uORB/Subscription.hpp>
+#include <uORB/topics/geofence_status.h>
 #include <uORB/topics/home_position.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/sensor_gps.h>
@@ -171,7 +172,7 @@ class Geofence : public ModuleParams
 	mission_stats_entry_s _stats;
 	DatamanState _dataman_state{DatamanState::UpdateRequestWait};
 	DatamanState _error_state{DatamanState::UpdateRequestWait};
-	DatamanCache _dataman_cache{"geofence_dm_cache_miss", 4};
+	DatamanCache _dataman_cache{"geofence_dm_cache_miss", 0};
 	DatamanClient	&_dataman_client = _dataman_cache.client();
 
 	float _altitude_min{0.0f};
@@ -185,6 +186,8 @@ class Geofence : public ModuleParams
 	bool _fence_updated{true};  ///< flag indicating if fence are updated to dataman cache
 	bool _initiate_fence_updated{true}; ///< flag indicating if fence updated is needed
 
+	uORB::Publication<geofence_status_s> _geofence_status_pub{ORB_ID(geofence_status)};
+
 	/**
 	 * implementation of updateFence()
 	 */