diff --git a/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp b/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp index 5720f6e52f..78980c8069 100644 --- a/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp +++ b/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp @@ -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]); diff --git a/src/modules/uORB/SubscriptionCallback.hpp b/src/modules/uORB/SubscriptionCallback.hpp index 9795f50a59..7f66d2396b 100644 --- a/src/modules/uORB/SubscriptionCallback.hpp +++ b/src/modules/uORB/SubscriptionCallback.hpp @@ -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};