d/vive: Update tracking status to fix segfault

Co-authored-by: Moses Turner <moses@collabora.com>

src/xrt/drivers/vive/vive_device.c

@@ -824,10 +824,9 @@ compute_distortion(struct xrt_device *xdev, int view, float u, float v, struct x
 	return u_compute_distortion_vive(&d->config.distortion[view], u, v, result);
 }
 
-static bool
+void
-vive_setup_trackers(struct vive_device *d, struct vive_tracking_status status)
+vive_set_trackers_status(struct vive_device *d, struct vive_tracking_status status)
 {
-	// Tracking status setup
 	bool dof3_enabled = true; // We always have at least 3dof HMD tracking
 	bool slam_wanted = status.slam_wanted;
 	bool slam_supported = status.slam_supported;
@@ -849,31 +848,15 @@ vive_setup_trackers(struct vive_device *d, struct vive_tracking_status status)
 	const char *slam_status = d->tracking.slam_enabled ? "Enabled"
 	                          : !slam_wanted           ? "Disabled by the user (envvar set to false)"
 	                          : !slam_supported        ? "Unavailable (not built)"
-	                                                   : NULL;
+	                                                   : "Failed to initialize";
 
 	const char *hand_status = d->tracking.hand_enabled ? "Enabled"
 	                          : !hand_wanted           ? "Disabled by the user (envvar set to false)"
 	                          : !hand_supported        ? "Unavailable (not built)"
-	                                                   : NULL;
+	                                                   : "Failed to initialize";
-
-	assert(slam_status != NULL && hand_status != NULL);
 
 	snprintf(d->gui.slam_status, sizeof(d->gui.slam_status), "%s", slam_status);
 	snprintf(d->gui.hand_status, sizeof(d->gui.hand_status), "%s", hand_status);
-
-	// Initialize 3DoF tracker
-	m_imu_3dof_init(&d->fusion.i3dof, M_IMU_3DOF_USE_GRAVITY_DUR_20MS);
-
-	int ret = os_mutex_init(&d->fusion.mutex);
-	if (ret != 0) {
-		VIVE_ERROR(d, "Failed to init 3dof mutex");
-		return false;
-	}
-
-	// SLAM tracker and hand tracker, if enabled, should've been initialized in
-	// the lighthouse builder. The out_sinks fields will be set there as well.
-
-	return true;
 }
 
 struct vive_device *
@@ -1037,11 +1020,15 @@ vive_device_create(struct os_hid_device *mainboard_dev,
 	}
 	snprintf(d->base.serial, XRT_DEVICE_NAME_LEN, "%s", d->config.firmware.device_serial_number);
 
-	bool trackers_set = vive_setup_trackers(d, tstatus);
+	vive_set_trackers_status(d, tstatus);
-	if (!trackers_set) {
+
-		VIVE_ERROR(d, "Failed to setup trackers");
+	// Initialize 3DoF tracker
-		vive_device_destroy((struct xrt_device *)d);
+	m_imu_3dof_init(&d->fusion.i3dof, M_IMU_3DOF_USE_GRAVITY_DUR_20MS);
-		return NULL;
+
+	ret = os_mutex_init(&d->fusion.mutex);
+	if (ret != 0) {
+		VIVE_ERROR(d, "Failed to init 3dof mutex");
+		return false;
 	}
 
 	ret = os_thread_helper_start(&d->sensors_thread, vive_sensors_run_thread, d);

src/xrt/drivers/vive/vive_device.h

@@ -127,6 +127,9 @@ struct vive_tracking_status
 	bool hand_enabled;
 };
 
+void
+vive_set_trackers_status(struct vive_device *d, struct vive_tracking_status status);
+
 struct vive_device *
 vive_device_create(struct os_hid_device *mainboard_dev,
                    struct os_hid_device *sensors_dev,

src/xrt/targets/common/target_builder_lighthouse.c

@@ -53,9 +53,8 @@
 //! @todo This should not be in static storage. Maybe make this inherit and replace usysd.
 static struct lighthouse_system
 {
-	bool use_libsurvive; //!< Whether we are using survive driver or vive driver
+	bool use_libsurvive;                      //!< Whether we are using survive driver or vive driver
-	bool slam_enabled;   //!< Whether SLAM tracking is enabled
+	struct vive_tracking_status vive_tstatus; //!< Status of tracking features for vive driver
-	bool hand_enabled;   //!< Whether hand tracking is enabled
 } lhs;
 
 DEBUG_GET_ONCE_LOG_OPTION(lh_log, "LH_LOG", U_LOGGING_WARN)
@@ -350,8 +349,8 @@ setup_visual_trackers(struct u_system_devices *usysd,
                       struct xrt_slam_sinks *out_sinks,
                       struct xrt_device **out_devices)
 {
-	bool slam_enabled = lhs.slam_enabled;
+	bool slam_enabled = lhs.vive_tstatus.slam_enabled;
-	bool hand_enabled = lhs.hand_enabled;
+	bool hand_enabled = lhs.vive_tstatus.hand_enabled;
 
 	struct t_stereo_camera_calibration *stereo_calib = NULL;
 	struct xrt_pose head_in_left_cam;
@@ -362,8 +361,9 @@ setup_visual_trackers(struct u_system_devices *usysd,
 	if (slam_enabled) {
 		slam_sinks = lighthouse_slam_track(usysd);
 		if (slam_sinks == NULL) {
+			lhs.vive_tstatus.slam_enabled = false;
+			slam_enabled = false;
 			LH_WARN("Unable to setup the SLAM tracker");
-			return false;
 		}
 	}
 
@@ -374,13 +374,19 @@ setup_visual_trackers(struct u_system_devices *usysd,
 		bool success =
 		    lighthouse_hand_track(usysd, xp, head_in_left_cam, stereo_calib, &hand_sinks, hand_devices);
 		if (!success) {
+			lhs.vive_tstatus.hand_enabled = false;
+			hand_enabled = false;
 			LH_WARN("Unable to setup the hand tracker");
-			return false;
 		}
 	}
 
 	t_stereo_camera_calibration_reference(&stereo_calib, NULL);
 
+	if (!lhs.use_libsurvive) { // Refresh trackers status in vive driver
+		struct vive_device *d = (struct vive_device *)usysd->base.roles.head;
+		vive_set_trackers_status(d, lhs.vive_tstatus);
+	}
+
 	// Setup frame graph
 
 	struct xrt_frame_sink *entry_left_sink = NULL;
@@ -452,7 +458,7 @@ stream_data_sources(struct u_system_devices *usysd, struct xrt_prober *xp, struc
 	uint32_t mode = get_selected_mode(finder.xfs);
 
 	// If SLAM is enabled (only on vive driver) we intercept the data sink
-	if (lhs.slam_enabled) {
+	if (lhs.vive_tstatus.slam_enabled) {
 		struct vive_device *d = (struct vive_device *)usysd->base.roles.head;
 		LH_ASSERT_(d != NULL && d->source != NULL);
 		struct vive_source *vs = d->source;
@@ -502,9 +508,15 @@ lighthouse_open_system(struct xrt_builder *xb,
 #endif
 	bool hand_enabled = hand_supported && hand_wanted;
 
-	lhs.slam_enabled = slam_enabled;
+	struct vive_tracking_status tstatus = {
-	lhs.hand_enabled = hand_enabled;
+	    .slam_wanted = slam_wanted,
-
+	    .slam_supported = slam_supported,
+	    .slam_enabled = slam_enabled,
+	    .hand_wanted = hand_wanted,
+	    .hand_supported = hand_supported,
+	    .hand_enabled = hand_enabled,
+	};
+	lhs.vive_tstatus = tstatus;
 
 	struct vive_config *hmd_config = NULL;
 
@@ -514,15 +526,6 @@ lighthouse_open_system(struct xrt_builder *xb,
 #endif
 	} else {
 #ifdef XRT_BUILD_DRIVER_VIVE
-		struct vive_tracking_status tstatus = {
-		    .slam_wanted = slam_wanted,
-		    .slam_supported = slam_supported,
-		    .slam_enabled = slam_enabled,
-		    .hand_wanted = hand_wanted,
-		    .hand_supported = hand_supported,
-		    .hand_enabled = hand_enabled,
-		};
-
 		struct xrt_prober_device **xpdevs = NULL;
 		size_t xpdev_count = 0;
 
@@ -544,8 +547,8 @@ lighthouse_open_system(struct xrt_builder *xb,
 			case VIVE_PRO_MAINBOARD_PID:
 			case VIVE_PRO_LHR_PID: {
 				struct vive_source *vs = vive_source_create(&usysd->xfctx);
-				int num_devices = vive_found(xp, xpdevs, xpdev_count, i, NULL, tstatus, vs, &hmd_config,
+				int num_devices = vive_found(xp, xpdevs, xpdev_count, i, NULL, lhs.vive_tstatus, vs,
-				                             &usysd->base.xdevs[usysd->base.xdev_count]);
+				                             &hmd_config, &usysd->base.xdevs[usysd->base.xdev_count]);
 				usysd->base.xdev_count += num_devices;
 
 			} break;