sensors/vehicle_gps_position: only register callbacks once topic published

- this avoids creating unnecessary uORB device nodes for GPS instances that might never exist
This commit is contained in:
Daniel Agar
2020-10-27 18:40:57 -04:00
parent df2f26ebdf
commit 91da194bd7
2 changed files with 25 additions and 23 deletions
@@ -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]);