geofence_update.msg: notify navigator on geofence update

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;