geofence_update.msg: notify navigator on geofence update

msg/CMakeLists.txt

@@ -59,6 +59,7 @@ set(msg_file_names
 	follow_target.msg
 	fw_pos_ctrl_status.msg
 	geofence_result.msg
+	geofence_update.msg
 	gps_dump.msg
 	gps_inject_data.msg
 	hil_sensor.msg

msg/geofence_update.msg

@@ -0,0 +1,4 @@
+# This message is used to notify the system about a geofence update (dataman
+# storage)
+
+uint32 dummy       # unused

src/modules/mavlink/mavlink_mission.cpp

@@ -51,6 +51,7 @@
 #include <px4_defines.h>
 
 #include <navigator/navigation.h>
+#include <uORB/topics/geofence_update.h>
 #include <uORB/topics/mission.h>
 #include <uORB/topics/mission_result.h>
 
@@ -87,6 +88,7 @@ MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) :
 	_offboard_mission_sub(-1),
 	_mission_result_sub(-1),
 	_offboard_mission_pub(nullptr),
+	_geofence_update_pub(nullptr),
 	_slow_rate_limiter(100 * 1000), // Rate limit sending of the current WP sequence to 10 Hz
 	_verbose(mavlink->verbose()),
 	_mavlink(mavlink)
@@ -101,6 +103,7 @@ MavlinkMissionManager::~MavlinkMissionManager()
 {
 	orb_unsubscribe(_mission_result_sub);
 	orb_unadvertise(_offboard_mission_pub);
+	orb_unadvertise(_geofence_update_pub);
 }
 
 void
@@ -210,7 +213,9 @@ MavlinkMissionManager::update_geofence_count(unsigned count)
 
 	if (res == sizeof(mission_stats_entry_s)) {
 		_count[(uint8_t)MAV_MISSION_TYPE_FENCE] = count;
-		// TODO: notify via orb
+
+		geofence_update_s geofence_update{};
+		orb_publish_auto(ORB_ID(geofence_update), &_geofence_update_pub, &geofence_update, nullptr, ORB_PRIO_DEFAULT);
 
 	} else {
 		warnx("WPM: ERROR: can't save mission state");

src/modules/mavlink/mavlink_mission.h

@@ -126,6 +126,7 @@ private:
 	int			_offboard_mission_sub;
 	int			_mission_result_sub;
 	orb_advert_t		_offboard_mission_pub;
+	orb_advert_t		_geofence_update_pub;
 
 	MavlinkRateLimiter	_slow_rate_limiter;
 

src/modules/navigator/navigator.h

@@ -236,6 +236,7 @@ private:
 	int		_param_update_sub{-1};		/**< param update subscription */
 	int		_sensor_combined_sub{-1};	/**< sensor combined subscription */
 	int		_vehicle_command_sub{-1};	/**< vehicle commands (onboard and offboard) */
+	int		_geofence_update_sub{-1};	/**< geofence updates */
 	int		_vstatus_sub{-1};		/**< vehicle status subscription */
 
 	orb_advert_t	_att_sp_pub{nullptr};
@@ -256,7 +257,7 @@ private:
 	vehicle_global_position_s			_global_pos{};		/**< global vehicle position */
 	vehicle_gps_position_s				_gps_pos{};		/**< gps position */
 	vehicle_land_detected_s				_land_detected{};	/**< vehicle land_detected */
-	vehicle_local_position_s			_local_pos;		/**< local vehicle position */
+	vehicle_local_position_s			_local_pos{};		/**< local vehicle position */
 	vehicle_status_s				_vstatus{};		/**< vehicle status */
 
 	int		_mission_instance_count{-1};	/**< instance count for the current mission */

src/modules/navigator/navigator_main.cpp

@@ -57,10 +57,15 @@
 #include <px4_tasks.h>
 #include <sys/ioctl.h>
 #include <sys/stat.h>
+
 #include <sys/types.h>
 #include <systemlib/mavlink_log.h>
 #include <systemlib/systemlib.h>
-#include <uORB/topics/fence.h>
+#include <drivers/device/device.h>
+#include <arch/board/board.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/geofence_update.h>
 #include <uORB/topics/fw_pos_ctrl_status.h>
 #include <uORB/topics/home_position.h>
 #include <uORB/topics/mission.h>
@@ -247,6 +252,7 @@ Navigator::task_main()
 	_offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission));
 	_param_update_sub = orb_subscribe(ORB_ID(parameter_update));
 	_vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command));
+	_geofence_update_sub = orb_subscribe(ORB_ID(geofence_update));
 
 	/* copy all topics first time */
 	vehicle_status_update();
@@ -340,6 +346,15 @@ Navigator::task_main()
 			params_update();
 		}
 
+		/* geofence updated */
+		orb_check(_geofence_update_sub, &updated);
+
+		if (updated) {
+			geofence_update_s geofence_update;
+			orb_copy(ORB_ID(geofence_update), _geofence_update_sub, &geofence_update);
+			_geofence.updateFence();
+		}
+
 		/* vehicle status updated */
 		orb_check(_vstatus_sub, &updated);
 
@@ -667,6 +682,33 @@ Navigator::task_main()
 		perf_end(_loop_perf);
 	}
 
+	orb_unsubscribe(_global_pos_sub);
+	_global_pos_sub = -1;
+	orb_unsubscribe(_local_pos_sub);
+	_local_pos_sub = -1;
+	orb_unsubscribe(_gps_pos_sub);
+	_gps_pos_sub = -1;
+	orb_unsubscribe(_sensor_combined_sub);
+	_sensor_combined_sub = -1;
+	orb_unsubscribe(_fw_pos_ctrl_status_sub);
+	_fw_pos_ctrl_status_sub = -1;
+	orb_unsubscribe(_vstatus_sub);
+	_vstatus_sub = -1;
+	orb_unsubscribe(_land_detected_sub);
+	_land_detected_sub = -1;
+	orb_unsubscribe(_home_pos_sub);
+	_home_pos_sub = -1;
+	orb_unsubscribe(_onboard_mission_sub);
+	_onboard_mission_sub = -1;
+	orb_unsubscribe(_offboard_mission_sub);
+	_offboard_mission_sub = -1;
+	orb_unsubscribe(_param_update_sub);
+	_param_update_sub = -1;
+	orb_unsubscribe(_vehicle_command_sub);
+	_vehicle_command_sub = -1;
+	orb_unsubscribe(_geofence_update_sub);
+	_geofence_update_sub = -1;
+
 	PX4_INFO("exiting");
 
 	_navigator_task = -1;