From 548f4edee3e6724b10efd28373222b1ee8c7de21 Mon Sep 17 00:00:00 2001
From: Jakob Bornecrantz <jakob@collabora.com>
Date: Sun, 15 Sep 2019 20:52:27 +0100
Subject: [PATCH] d/psmv: Don't use the copy sample on ZCM2

---
 src/xrt/drivers/psmv/psmv_driver.c | 150 ++++++++++++++++++-----------
 1 file changed, 94 insertions(+), 56 deletions(-)

diff --git a/src/xrt/drivers/psmv/psmv_driver.c b/src/xrt/drivers/psmv/psmv_driver.c
index 7fa8496c6..bf05a8ac9 100644
--- a/src/xrt/drivers/psmv/psmv_driver.c
+++ b/src/xrt/drivers/psmv/psmv_driver.c
@@ -318,7 +318,6 @@ struct psmv_parsed_sample
 {
 	struct xrt_vec3_i32 accel;
 	struct xrt_vec3_i32 gyro;
-	uint8_t trigger;
 };
 
 /*!
@@ -331,7 +330,34 @@ struct psmv_parsed_input
 	uint8_t battery;
 	uint8_t seq_no;
 
-	struct psmv_parsed_sample sample[2];
+
+	union {
+		//! Trigger for the last two frames (ZCM1).
+		uint8_t trigger_values[2];
+
+		struct
+		{
+			//! Low-pass filtered versio of trigger (ZCM2).
+			uint8_t trigger_low_pass;
+
+			//! Trigger (ZCM2).
+			uint8_t trigger;
+		};
+	};
+
+	union {
+		//! Accelerometer and gyro scope samples (ZCM1).
+		struct psmv_parsed_sample samples[2];
+
+		struct
+		{
+			//! Accelerometer and gyro scope samples (ZCM2).
+			struct psmv_parsed_sample sample;
+
+			//! Copy of above (ZCM2).
+			struct psmv_parsed_sample sample_copy;
+		};
+	};
 };
 
 /*!
@@ -490,7 +516,7 @@ struct psmv_device
 static int
 psmv_get_calibration(struct psmv_device *psmv);
 
-static void
+static int
 psmv_parse_input(struct psmv_device *psmv,
                  void *data,
                  struct psmv_parsed_input *input);
@@ -551,8 +577,7 @@ static void
 psmv_update_trigger_value(struct psmv_device *psmv, int index, int64_t now)
 {
 	psmv->base.inputs[index].timestamp = now;
-	psmv->base.inputs[index].value.vec1.x =
-	    psmv->last.sample[1].trigger / 255.0f;
+	psmv->base.inputs[index].value.vec1.x = psmv->last.trigger / 255.0f;
 }
 
 
@@ -653,7 +678,7 @@ psmv_run_thread(void *ptr)
 
 		timepoint_ns now_ns = time_state_get_now(time);
 
-		psmv_parse_input(psmv, data.buffer, &input);
+		int num = psmv_parse_input(psmv, data.buffer, &input);
 
 		float dt = time_ns_to_s(now_ns - then_ns);
 		then_ns = now_ns;
@@ -665,8 +690,16 @@ psmv_run_thread(void *ptr)
 		psmv->last = input;
 
 		// Process the parsed data.
-		update_fusion(psmv, &input.sample[0], dt / 2.0);
-		update_fusion(psmv, &input.sample[1], dt / 2.0);
+		if (num == 2) {
+			// ZCM1
+			update_fusion(psmv, &input.samples[0], dt / 2.0);
+			update_fusion(psmv, &input.samples[1], dt / 2.0);
+		} else if (num == 1) {
+			// ZCM2
+			update_fusion(psmv, &input.sample, dt);
+		} else {
+			assert(false);
+		}
 
 		// Now done.
 		os_mutex_unlock(&psmv->lock);
@@ -970,10 +1003,10 @@ psmv_found(struct xrt_prober *xp,
 	u_var_add_vec3_f32(psmv, &psmv->calibration.gyro.factor, "gyro.factor");
 	u_var_add_vec3_f32(psmv, &psmv->calibration.gyro.bias, "gyro.bias");
 	u_var_add_gui_header(psmv, &psmv->gui.last_frame, "Last data");
-	u_var_add_ro_vec3_i32(psmv, &psmv->last.sample[0].accel, "last.sample[0].accel");
-	u_var_add_ro_vec3_i32(psmv, &psmv->last.sample[1].accel, "last.sample[1].accel");
-	u_var_add_ro_vec3_i32(psmv, &psmv->last.sample[0].gyro, "last.sample[0].gyro");
-	u_var_add_ro_vec3_i32(psmv, &psmv->last.sample[1].gyro, "last.sample[1].gyro");
+	u_var_add_ro_vec3_i32(psmv, &psmv->last.samples[0].accel, "last.samples[0].accel");
+	u_var_add_ro_vec3_i32(psmv, &psmv->last.samples[1].accel, "last.samples[1].accel");
+	u_var_add_ro_vec3_i32(psmv, &psmv->last.samples[0].gyro, "last.samples[0].gyro");
+	u_var_add_ro_vec3_i32(psmv, &psmv->last.samples[1].gyro, "last.samples[1].gyro");
 	u_var_add_ro_vec3_f32(psmv, &psmv->read.accel, "read.accel");
 	u_var_add_ro_vec3_f32(psmv, &psmv->read.gyro, "read.gyro");
 	u_var_add_gui_header(psmv, &psmv->gui.control, "Control");
@@ -1213,7 +1246,7 @@ psmv_get_calibration_zcm1(struct psmv_device *psmv)
 	return 0;
 }
 
-static void
+static int
 psmv_parse_input_zcm1(struct psmv_device *psmv,
                       struct psmv_input_zcm1 *data,
                       struct psmv_parsed_input *input)
@@ -1228,13 +1261,14 @@ psmv_parse_input_zcm1(struct psmv_device *psmv,
 	input->timestamp |= data->timestamp_low;
 	input->timestamp |= data->timestamp_high << 8;
 
-	input->sample[0].trigger = data->trigger_f1;
-	psmv_from_vec3_u16_wire(&input->sample[0].accel, &data->accel_f1);
-	psmv_from_vec3_u16_wire(&input->sample[0].gyro, &data->gyro_f1);
+	input->trigger_values[0] = data->trigger_f1;
+	input->trigger_values[1] = data->trigger_f2;
 
-	input->sample[1].trigger = data->trigger_f2;
-	psmv_from_vec3_u16_wire(&input->sample[1].accel, &data->accel_f2);
-	psmv_from_vec3_u16_wire(&input->sample[1].gyro, &data->gyro_f2);
+	psmv_from_vec3_u16_wire(&input->samples[0].accel, &data->accel_f1);
+	psmv_from_vec3_u16_wire(&input->samples[0].gyro, &data->gyro_f1);
+
+	psmv_from_vec3_u16_wire(&input->samples[1].accel, &data->accel_f2);
+	psmv_from_vec3_u16_wire(&input->samples[1].gyro, &data->gyro_f2);
 
 	uint32_t diff = psmv_calc_delta_and_handle_rollover(
 	    input->timestamp, psmv->last.timestamp);
@@ -1250,24 +1284,26 @@ psmv_parse_input_zcm1(struct psmv_device *psmv,
 	          "missed: %s\n\t"
 	          "buttons: %08x\n\t"
 	          "battery: %x\n\t"
-	          "sample[0].accel: %6i %6i %6i\n\t"
-	          "sample[1].accel: %6i %6i %6i\n\t"
-	          "sample[0].gyro:  %6i %6i %6i\n\t"
-	          "sample[1].gyro:  %6i %6i %6i\n\t"
-	          "sample[0].trigger: %02x\n\t"
-	          "sample[1].trigger: %02x\n\t"
+	          "samples[0].accel: %6i %6i %6i\n\t"
+	          "samples[1].accel: %6i %6i %6i\n\t"
+	          "samples[0].gyro:  %6i %6i %6i\n\t"
+	          "samples[1].gyro:  %6i %6i %6i\n\t"
+	          "trigger_values[0]: %02x\n\t"
+	          "trigger_values[1]: %02x\n\t"
 	          "timestamp: %i\n\t"
 	          "diff: %i\n\t"
 	          "seq_no: %x\n",
 	          missed ? "yes" : "no", input->buttons, input->battery,
-	          input->sample[0].accel.x, input->sample[0].accel.y,
-	          input->sample[0].accel.z, input->sample[1].accel.x,
-	          input->sample[1].accel.y, input->sample[1].accel.z,
-	          input->sample[0].gyro.x, input->sample[0].gyro.y,
-	          input->sample[0].gyro.z, input->sample[1].gyro.x,
-	          input->sample[1].gyro.y, input->sample[1].gyro.z,
-	          input->sample[0].trigger, input->sample[1].trigger,
+	          input->samples[0].accel.x, input->samples[0].accel.y,
+	          input->samples[0].accel.z, input->samples[1].accel.x,
+	          input->samples[1].accel.y, input->samples[1].accel.z,
+	          input->samples[0].gyro.x, input->samples[0].gyro.y,
+	          input->samples[0].gyro.z, input->samples[1].gyro.x,
+	          input->samples[1].gyro.y, input->samples[1].gyro.z,
+	          input->trigger_values[0], input->trigger_values[1],
 	          input->timestamp, diff, input->seq_no);
+
+	return 2;
 }
 
 
@@ -1430,7 +1466,7 @@ psmv_get_calibration_zcm2(struct psmv_device *psmv)
 	return 0;
 }
 
-static void
+static int
 psmv_parse_input_zcm2(struct psmv_device *psmv,
                       struct psmv_input_zcm2 *data,
                       struct psmv_parsed_input *input)
@@ -1445,14 +1481,14 @@ psmv_parse_input_zcm2(struct psmv_device *psmv,
 	input->timestamp |= data->timestamp_low;
 	input->timestamp |= data->timestamp_high << 8;
 
-	input->sample[0].trigger = data->trigger_low_pass;
-	input->sample[1].trigger = data->trigger;
+	input->trigger_low_pass = data->trigger_low_pass;
+	input->trigger = data->trigger;
 
-	psmv_from_vec3_i16_wire(&input->sample[0].accel, &data->accel);
-	psmv_from_vec3_i16_wire(&input->sample[0].gyro, &data->gyro);
+	psmv_from_vec3_i16_wire(&input->sample.accel, &data->accel);
+	psmv_from_vec3_i16_wire(&input->sample.gyro, &data->gyro);
 
-	psmv_from_vec3_i16_wire(&input->sample[1].accel, &data->accel_copy);
-	psmv_from_vec3_i16_wire(&input->sample[1].gyro, &data->gyro_copy);
+	psmv_from_vec3_i16_wire(&input->sample_copy.accel, &data->accel_copy);
+	psmv_from_vec3_i16_wire(&input->sample_copy.gyro, &data->gyro_copy);
 
 	uint32_t diff = psmv_calc_delta_and_handle_rollover(
 	    input->timestamp, psmv->last.timestamp);
@@ -1468,24 +1504,26 @@ psmv_parse_input_zcm2(struct psmv_device *psmv,
 	          "missed: %s\n\t"
 	          "buttons: %08x\n\t"
 	          "battery: %x\n\t"
-	          "sample[0].accel: %6i %6i %6i\n\t"
-	          "sample[1].accel: %6i %6i %6i\n\t"
-	          "sample[0].gyro:  %6i %6i %6i\n\t"
-	          "sample[1].gyro:  %6i %6i %6i\n\t"
-	          "sample.trigger_low_pass: %02x\n\t"
+	          "sample.accel:      %6i %6i %6i\n\t"
+	          "sample_copy.accel: %6i %6i %6i\n\t"
+	          "sample.gyro:       %6i %6i %6i\n\t"
+	          "sample_copy.gyro:  %6i %6i %6i\n\t"
 	          "sample.trigger: %02x\n\t"
+	          "sample.trigger_low_pass: %02x\n\t"
 	          "timestamp: %i\n\t"
 	          "diff: %i\n\t"
 	          "seq_no: %x\n",
 	          missed ? "yes" : "no", input->buttons, input->battery,
-	          input->sample[0].accel.x, input->sample[0].accel.y,
-	          input->sample[0].accel.z, input->sample[1].accel.x,
-	          input->sample[1].accel.y, input->sample[1].accel.z,
-	          input->sample[0].gyro.x, input->sample[0].gyro.y,
-	          input->sample[0].gyro.z, input->sample[1].gyro.x,
-	          input->sample[1].gyro.y, input->sample[1].gyro.z,
-	          input->sample[0].trigger, input->sample[1].trigger,
-	          input->timestamp, diff, input->seq_no);
+	          input->samples[0].accel.x, input->samples[0].accel.y,
+	          input->samples[0].accel.z, input->samples[1].accel.x,
+	          input->samples[1].accel.y, input->samples[1].accel.z,
+	          input->samples[0].gyro.x, input->samples[0].gyro.y,
+	          input->samples[0].gyro.z, input->samples[1].gyro.x,
+	          input->samples[1].gyro.y, input->samples[1].gyro.z,
+	          input->trigger_low_pass, input->trigger, input->timestamp,
+	          diff, input->seq_no);
+
+	return 1;
 }
 
 
@@ -1507,15 +1545,15 @@ psmv_get_calibration(struct psmv_device *psmv)
 	return 0;
 }
 
-static void
+static int
 psmv_parse_input(struct psmv_device *psmv,
                  void *data,
                  struct psmv_parsed_input *input)
 {
 	switch (psmv->pid) {
-	case PSMV_PID_ZCM1: psmv_parse_input_zcm1(psmv, data, input); break;
-	case PSMV_PID_ZCM2: psmv_parse_input_zcm2(psmv, data, input); break;
-	default: break;
+	case PSMV_PID_ZCM1: return psmv_parse_input_zcm1(psmv, data, input);
+	case PSMV_PID_ZCM2: return psmv_parse_input_zcm2(psmv, data, input);
+	default: return 0;
 	}
 }