From 815cea2abbb3399b7de7140773b295ed48adf44e Mon Sep 17 00:00:00 2001
From: Konrad <konrad@auterion.com>
Date: Wed, 6 Mar 2024 10:40:34 +0100
Subject: [PATCH] geofence: publish status of loaded geofence

---
 msg/CMakeLists.txt                 |  1 +
 msg/GeofenceStatus.msg             |  7 +++++++
 src/modules/navigator/geofence.cpp | 25 +++++++++++++++++++++++++
 src/modules/navigator/geofence.h   |  5 ++++-
 4 files changed, 37 insertions(+), 1 deletion(-)
 create mode 100644 msg/GeofenceStatus.msg

diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt
index 62544cffc0..2f8bb98c80 100644
--- a/msg/CMakeLists.txt
+++ b/msg/CMakeLists.txt
@@ -99,6 +99,7 @@ set(msg_files
 	FollowTargetStatus.msg
 	GeneratorStatus.msg
 	GeofenceResult.msg
+	GeofenceStatus.msg
 	GimbalControls.msg
 	GimbalDeviceAttitudeStatus.msg
 	GimbalDeviceInformation.msg
diff --git a/msg/GeofenceStatus.msg b/msg/GeofenceStatus.msg
new file mode 100644
index 0000000000..d32b9010c3
--- /dev/null
+++ b/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
diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp
index 4f48d768a5..55917331a6 100644
--- a/src/modules/navigator/geofence.cpp
+++ b/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;
diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h
index f2f057dc9a..71d0a6bdf7 100644
--- a/src/modules/navigator/geofence.h
+++ b/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 @@ private:
 	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 @@ private:
 	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()
 	 */