dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
msg/CMakeLists.txt
View File

@ -99,6 +99,7 @@ set(msg_files
FollowTargetStatus.msg
GeneratorStatus.msg
GeofenceResult.msg
GeofenceStatus.msg
GimbalControls.msg
GimbalDeviceAttitudeStatus.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

25
src/modules/navigator/geofence.cpp
View File

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

5
src/modules/navigator/geofence.h
View File

@ -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()
*/