geofence: publish status of loaded geofence

This commit is contained in:
Konrad 2024-03-06 10:40:34 +01:00 committed by Silvan Fuhrer
parent 51321c605e
commit 815cea2abb
4 changed files with 37 additions and 1 deletions

View File

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

7
msg/GeofenceStatus.msg Normal file
View File

@ -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

View File

@ -81,6 +81,8 @@ Geofence::Geofence(Navigator *navigator) :
if (_navigator != nullptr) { if (_navigator != nullptr) {
updateFence(); updateFence();
} }
_geofence_status_pub.advertise();
} }
Geofence::~Geofence() Geofence::~Geofence()
@ -101,6 +103,14 @@ void Geofence::run()
if (_initiate_fence_updated) { if (_initiate_fence_updated) {
_initiate_fence_updated = false; _initiate_fence_updated = false;
_dataman_state = DatamanState::Read; _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; break;
@ -147,6 +157,14 @@ void Geofence::run()
} else { } else {
_dataman_state = DatamanState::UpdateRequestWait; _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; _dataman_state = DatamanState::UpdateRequestWait;
_updateFence(); _updateFence();
_fence_updated = true; _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; break;

View File

@ -49,6 +49,7 @@
#include <lib/geo/geo.h> #include <lib/geo/geo.h>
#include <px4_platform_common/defines.h> #include <px4_platform_common/defines.h>
#include <uORB/Subscription.hpp> #include <uORB/Subscription.hpp>
#include <uORB/topics/geofence_status.h>
#include <uORB/topics/home_position.h> #include <uORB/topics/home_position.h>
#include <uORB/topics/vehicle_global_position.h> #include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/sensor_gps.h> #include <uORB/topics/sensor_gps.h>
@ -171,7 +172,7 @@ private:
mission_stats_entry_s _stats; mission_stats_entry_s _stats;
DatamanState _dataman_state{DatamanState::UpdateRequestWait}; DatamanState _dataman_state{DatamanState::UpdateRequestWait};
DatamanState _error_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(); DatamanClient &_dataman_client = _dataman_cache.client();
float _altitude_min{0.0f}; float _altitude_min{0.0f};
@ -185,6 +186,8 @@ private:
bool _fence_updated{true}; ///< flag indicating if fence are updated to dataman cache 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 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() * implementation of updateFence()
*/ */