fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

sensors/vehicle_gps_position: only register callbacks once topic published

Browse Source 
- this avoids creating unnecessary uORB device nodes for GPS instances that might never exist
This commit is contained in:
Daniel Agar 2020-10-27 14:30:44 -04:00
parent df2f26ebdf
commit 91da194bd7
2 changed files with 25 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp
View File

@ -59,16 +59,9 @@ bool VehicleGPSPosition::Start()
// force initial updates
ParametersUpdate(true);
// needed to change the active sensor if the primary stops updating
bool anyGpsRegistered = false;
bool registered[GPS_MAX_RECEIVERS];
ScheduleNow();
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
registered[i] = _sensor_gps_sub[i].registerCallback();
anyGpsRegistered |= registered[i];
}
return anyGpsRegistered;
return true;
}
void VehicleGPSPosition::Stop()
@ -98,21 +91,26 @@ void VehicleGPSPosition::Run()
perf_begin(_cycle_perf);
ParametersUpdate();
// backup schedule
ScheduleDelayed(500_ms);
// Check all GPS instance
bool any_gps_updated = false;
bool gps_updated[GPS_MAX_RECEIVERS];
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
gps_updated[i] = _sensor_gps_sub[i].updated();
if (gps_updated[i]) {
any_gps_updated = true;
_sensor_gps_sub[i].copy(&_gps_state[i]);
if (!_sensor_gps_sub[i].registered()) {
_sensor_gps_sub[i].registerCallback();
}
}
}
if ((_param_sens_gps_mask.get() == 0) && gps_updated[0]) {
// When GPS blending is disabled we always use the first receiver instance
Publish(_gps_state[0]);

28
src/modules/uORB/SubscriptionCallback.hpp
View File

@ -68,22 +68,24 @@ public:
bool registerCallback()
{
if (_subscription.get_node() && _subscription.get_node()->register_callback(this)) {
// registered
_registered = true;
if (!_registered) {
if (_subscription.get_node() && _subscription.get_node()->register_callback(this)) {
// registered
_registered = true;
} else {
// force topic creation by subscribing with old API
int fd = orb_subscribe_multi(_subscription.get_topic(), _subscription.get_instance());
} else {
// force topic creation by subscribing with old API
int fd = orb_subscribe_multi(_subscription.get_topic(), _subscription.get_instance());
// try to register callback again
if (_subscription.subscribe()) {
if (_subscription.get_node() && _subscription.get_node()->register_callback(this)) {
_registered = true;
// try to register callback again
if (_subscription.subscribe()) {
if (_subscription.get_node() && _subscription.get_node()->register_callback(this)) {
_registered = true;
}
}
}
orb_unsubscribe(fd);
orb_unsubscribe(fd);
}
}
return _registered;
@ -131,6 +133,8 @@ public:
virtual void call() = 0;
bool registered() const { return _registered; }
protected:
bool _registered{false};