Merge pull request #73 from ThibG/master

Fix initial accelerometer-based tilt correction. Fixes slight Oculus CV1 drift issues.

src/drv_oculus_rift/packet.c

@@ -107,11 +107,12 @@ bool decode_tracker_sensor_msg(pkt_tracker_sensor* msg, const unsigned char* buf
 	SKIP_CMD;
 	msg->num_samples = READ8;
 	msg->timestamp = READ16;
+	msg->timestamp *= 1000; // DK1 timestamps are in milliseconds
 	msg->last_command_id = READ16;
 	msg->temperature = READ16;
 
-	int actual = OHMD_MIN(msg->num_samples, 3);
-	for(int i = 0; i < actual; i++){
+	msg->num_samples = OHMD_MIN(msg->num_samples, 3);
+	for(int i = 0; i < msg->num_samples; i++){
 		decode_sample(buffer, msg->samples[i].accel);
 		buffer += 8;
 
@@ -120,7 +121,7 @@ bool decode_tracker_sensor_msg(pkt_tracker_sensor* msg, const unsigned char* buf
 	}
 
 	// Skip empty samples
-	buffer += (3 - actual) * 16;
+	buffer += (3 - msg->num_samples) * 16;
 	for(int i = 0; i < 3; i++){
 		msg->mag[i] = READ16;
 	}
@@ -138,12 +139,16 @@ bool decode_tracker_sensor_msg_dk2(pkt_tracker_sensor* msg, const unsigned char*
 	SKIP_CMD;
 	msg->last_command_id = READ16;
 	msg->num_samples = READ8;
+	/* Next is the number of samples since start, excluding the samples
+	contained in this packet */
 	buffer += 2; // unused: nb_samples_since_start
 	msg->temperature = READ16;
 	msg->timestamp = READ32;
 
-	int actual = OHMD_MIN(msg->num_samples, 2);
-	for(int i = 0; i < actual; i++){
+	/* Second sample value is junk (outdated/uninitialized) value if
+	num_samples < 2. */
+	msg->num_samples = OHMD_MIN(msg->num_samples, 2);
+	for(int i = 0; i < msg->num_samples; i++){
 		decode_sample(buffer, msg->samples[i].accel);
 		buffer += 8;
 
@@ -152,7 +157,7 @@ bool decode_tracker_sensor_msg_dk2(pkt_tracker_sensor* msg, const unsigned char*
 	}
 
 	// Skip empty samples
-	buffer += (2 - actual) * 16;
+	buffer += (2 - msg->num_samples) * 16;
 
 	for(int i = 0; i < 3; i++){
 		msg->mag[i] = READ16;
@@ -262,7 +267,7 @@ void dump_packet_tracker_sensor(const pkt_tracker_sensor* sensor)
 	LOGD("  num samples:     %u", sensor->num_samples);
 	LOGD("  magnetic field:  %i %i %i", sensor->mag[0], sensor->mag[1], sensor->mag[2]);
 
-	for(int i = 0; i < OHMD_MIN(sensor->num_samples, 3); i++){
+	for(int i = 0; i < sensor->num_samples; i++){
 		LOGD("    accel: %d %d %d", sensor->samples[i].accel[0], sensor->samples[i].accel[1], sensor->samples[i].accel[2]);
 		LOGD("    gyro:  %d %d %d", sensor->samples[i].gyro[0], sensor->samples[i].gyro[1], sensor->samples[i].gyro[2]);
 	}

src/drv_oculus_rift/rift.c

@@ -29,6 +29,7 @@ typedef struct {
 	rift_coordinate_frame coordinate_frame, hw_coordinate_frame;
 	pkt_sensor_config sensor_config;
 	pkt_tracker_sensor sensor;
+	uint32_t last_imu_timestamp;
 	double last_keep_alive;
 	fusion sensor_fusion;
 	vec3f raw_mag, raw_accel, raw_gyro;
@@ -112,22 +113,26 @@ static void handle_tracker_sensor_msg(rift_priv* priv, unsigned char* buffer, in
 
 	dump_packet_tracker_sensor(s);
 
-	// TODO handle missed samples etc.
-
-	float dt = s->num_samples > 3 ? (s->num_samples - 2) * TICK_LEN : TICK_LEN;
-
 	int32_t mag32[] = { s->mag[0], s->mag[1], s->mag[2] };
 	vec3f_from_rift_vec(mag32, &priv->raw_mag);
 
-	for(int i = 0; i < OHMD_MIN(s->num_samples, 3); i++){
+	// TODO: handle overflows in a nicer way
+	float dt = TICK_LEN; // TODO: query the Rift for the sample rate
+	if (s->timestamp > priv->last_imu_timestamp)
+	{
+		dt = (s->timestamp - priv->last_imu_timestamp) / 1000000.0f;
+		dt -= (s->num_samples - 1) * TICK_LEN; // TODO: query the Rift for the sample rate
+	}
+
+	for(int i = 0; i < s->num_samples; i++){
 		vec3f_from_rift_vec(s->samples[i].accel, &priv->raw_accel);
 		vec3f_from_rift_vec(s->samples[i].gyro, &priv->raw_gyro);
 
 		ofusion_update(&priv->sensor_fusion, dt, &priv->raw_gyro, &priv->raw_accel, &priv->raw_mag);
-
-		// reset dt to tick_len for the last samples if there were more than one sample
-		dt = TICK_LEN;
+		dt = TICK_LEN; // TODO: query the Rift for the sample rate
 	}
+
+	priv->last_imu_timestamp = s->timestamp;
 }
 
 static void update_device(ohmd_device* device)
@@ -226,6 +231,8 @@ static ohmd_device* open_device(ohmd_driver* driver, ohmd_device_desc* desc)
 	if(!priv)
 		goto cleanup;
 
+	priv->last_imu_timestamp = -1;
+
 	priv->base.ctx = driver->ctx;
 
 	// Open the HID device

src/drv_oculus_rift/rift.h

@@ -63,7 +63,7 @@ typedef struct {
 
 typedef struct {
 	uint8_t num_samples;
-	uint16_t timestamp;
+	uint32_t timestamp;
 	uint16_t last_command_id;
 	int16_t temperature;
 	pkt_tracker_sample samples[3];

src/fusion.c

@@ -64,7 +64,7 @@ void ofusion_update(fusion* me, float dt, const vec3f* ang_vel, const vec3f* acc
 		// otherwise reset the counter and start over
 
 		me->device_level_count =
-			fabsf(ovec3f_get_length(accel) - 9.82f) < gravity_tolerance && ang_vel_length < ang_vel_tolerance
+			fabsf(ovec3f_get_length(accel) - 9.82f) < gravity_tolerance * 2.0f && ang_vel_length < ang_vel_tolerance
 			? me->device_level_count + 1 : 0;
 
 		// device has been level for long enough, grab mean from the accelerometer filter queue (last n values)
@@ -75,22 +75,24 @@ void ofusion_update(fusion* me, float dt, const vec3f* ang_vel, const vec3f* acc
 
 			vec3f accel_mean;
 			ofq_get_mean(&me->accel_fq, &accel_mean);
-
-			// Calculate a cross product between what the device
-			// thinks is up and what gravity indicates is down.
-			// The values are optimized of what we would get out
-			// from the cross product.
-			vec3f tilt = {{accel_mean.z, 0, -accel_mean.x}};
-
-			ovec3f_normalize_me(&tilt);
-			ovec3f_normalize_me(&accel_mean);
-
-			vec3f up = {{0, 1.0f, 0}};
-			float tilt_angle = ovec3f_get_angle(&up, &accel_mean);
-
-			if(tilt_angle > max_tilt_error){
-				me->grav_error_angle = tilt_angle;
-				me->grav_error_axis = tilt;
+			if (ovec3f_get_length(&accel_mean) - 9.82f < gravity_tolerance)
+			{
+				// Calculate a cross product between what the device
+				// thinks is up and what gravity indicates is down.
+				// The values are optimized of what we would get out
+				// from the cross product.
+				vec3f tilt = {{accel_mean.z, 0, -accel_mean.x}};
+
+				ovec3f_normalize_me(&tilt);
+				ovec3f_normalize_me(&accel_mean);
+
+				vec3f up = {{0, 1.0f, 0}};
+				float tilt_angle = ovec3f_get_angle(&up, &accel_mean);
+
+				if(tilt_angle > max_tilt_error){
+					me->grav_error_angle = tilt_angle;
+					me->grav_error_axis = tilt;
+				}
 			}
 		}
 
@@ -98,7 +100,7 @@ void ofusion_update(fusion* me, float dt, const vec3f* ang_vel, const vec3f* acc
 		if(me->grav_error_angle > min_tilt_error){
 			float use_angle;
 			// if less than 2000 iterations have passed, set the up axis to the correction value outright
-			if(me->grav_error_angle > gravity_tolerance && me->iterations < 2000){
+			if(me->iterations < 2000){
 				use_angle = -me->grav_error_angle;
 				me->grav_error_angle = 0;
 			}
@@ -121,4 +123,4 @@ void ofusion_update(fusion* me, float dt, const vec3f* ang_vel, const vec3f* acc
 	// mitigate drift due to floating point
 	// inprecision with quat multiplication.
 	oquatf_normalize_me(&me->orient);
-}
+}