From 76a8617529af2b5268669e5d4487e29249a43788 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Thu, 17 Jun 2021 10:38:33 -0400
Subject: [PATCH] sensors: fix IMU init race condition

 - IMU init requires valid published data (device ids, etc)
 - orb_group_count will include advertised instances before data is published, so this can't be used to throttle IMU init attempts
---
 src/modules/sensors/sensors.cpp | 9 ++++++---
 1 file changed, 6 insertions(+), 3 deletions(-)

diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 28afeb746d..7ffd6d7246 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -665,7 +665,7 @@ void Sensors::Run()
 
 	// keep adding sensors as long as we are not armed,
 	// when not adding sensors poll for param updates
-	if (!_armed && hrt_elapsed_time(&_last_config_update) > 500_ms) {
+	if (!_armed && hrt_elapsed_time(&_last_config_update) > 1000_ms) {
 
 		const int n_accel = orb_group_count(ORB_ID(sensor_accel));
 		const int n_baro  = orb_group_count(ORB_ID(sensor_baro));
@@ -679,15 +679,18 @@ void Sensors::Run()
 			_n_gps = n_gps;
 			_n_gyro = n_gyro;
 			_n_mag = n_mag;
+
 			parameters_update();
 
-			_voted_sensors_update.initializeSensors();
 			InitializeVehicleAirData();
-			InitializeVehicleIMU();
 			InitializeVehicleGPSPosition();
 			InitializeVehicleMagnetometer();
 		}
 
+		// sensor device id (not just orb_group_count) must be populated before IMU init can succeed
+		_voted_sensors_update.initializeSensors();
+		InitializeVehicleIMU();
+
 		_last_config_update = hrt_absolute_time();
 
 	} else {