Merge branch 'master' of https://github.com/cnlohr/libsurvive

3 files changed, 27 insertions, 21 deletions

diff --git a/src/poser_sba.c b/src/poser_sba.c
index fcf4f2e..23e03fc 100644
--- a/src/poser_sba.c
+++ b/src/poser_sba.c
@@ -399,7 +399,7 @@ int PoserSBA(SurviveObject *so, PoserData *pd) {
 		d->failures_to_reset_cntr = 0;
 		d->failures_to_reset = survive_configi(ctx, "sba-failures-to-reset", SC_GET, 1);
 		d->successes_to_reset_cntr = 0;
-		d->successes_to_reset = survive_configi(ctx, "sba-successes-to-reset", SC_GET, 100);
+		d->successes_to_reset = survive_configi(ctx, "sba-successes-to-reset", SC_GET, -1);
 		d->useIMU = survive_configi(ctx, "sba-use-imu", SC_GET, 1);
 		d->required_meas = survive_configi(ctx, "sba-required-meas", SC_GET, 8);
 		d->max_error = survive_configf(ctx, "sba-max-error", SC_GET, .0001);
@@ -435,27 +435,28 @@ int PoserSBA(SurviveObject *so, PoserData *pd) {
 
 			d->last_lh = lightData->lh;
 			d->last_acode = lightData->acode;
-		}
 
-		if (error < 0) {
-			if (d->failures_to_reset_cntr > 0)
-				d->failures_to_reset_cntr--;
-		} else {
-			if (d->useIMU) {
-				FLT var_meters = 0.5;
-				FLT var_quat = error + .05;
-				FLT var[7] = {error * var_meters, error * var_meters, error * var_meters, error * var_quat,
-							  error * var_quat,   error * var_quat,   error * var_quat};
-
-				survive_imu_tracker_integrate_observation(so, lightData->timecode, &d->tracker, &estimate, var);
-				estimate = d->tracker.pose;
+
+			if (error < 0) {
+				if (d->failures_to_reset_cntr > 0)
+					d->failures_to_reset_cntr--;
 			}
+			else {
+				if (d->useIMU) {
+					FLT var_meters = 0.5;
+					FLT var_quat = error + .05;
+					FLT var[7] = { error * var_meters, error * var_meters, error * var_meters, error * var_quat,
+								  error * var_quat,   error * var_quat,   error * var_quat };
+
+					survive_imu_tracker_integrate_observation(so, lightData->timecode, &d->tracker, &estimate, var);
+					estimate = d->tracker.pose;
+				}
 
-			PoserData_poser_pose_func(&lightData->hdr, so, &estimate);
-			if (d->successes_to_reset_cntr > 0)
-				d->successes_to_reset_cntr--;
+				PoserData_poser_pose_func(&lightData->hdr, so, &estimate);
+				if (d->successes_to_reset_cntr > 0)
+					d->successes_to_reset_cntr--;
+			}
 		}
-
 		return 0;
 	}
 	case POSERDATA_FULL_SCENE: {
diff --git a/src/survive.c b/src/survive.c
index b024bbb..9e750f9 100644
--- a/src/survive.c
+++ b/src/survive.c
@@ -106,7 +106,7 @@ SurviveContext *survive_init_internal(int argc, char *const *argv) {
 	static int did_manual_driver_registration = 0;
 	if (did_manual_driver_registration == 0) {
 #define MANUAL_DRIVER_REGISTRATION(func)                                                                               \
-	int func(SurviveObject *so, PoserData *pd);                                                                        \
+ 	int func(SurviveObject *so, PoserData *pd);                                                                        \
 	RegisterDriver(#func, &func);
 
 		MANUAL_DRIVER_REGISTRATION(PoserCharlesSlow)
diff --git a/src/survive_imu.c b/src/survive_imu.c
index 36d1aeb..8f4266a 100644
--- a/src/survive_imu.c
+++ b/src/survive_imu.c
@@ -174,7 +174,7 @@ static void iterate_velocity(LinmathVec3d result, SurviveIMUTracker *tracker, do
 }
 
 void survive_imu_tracker_integrate(SurviveObject *so, SurviveIMUTracker *tracker, PoserDataIMU *data) {
-	if (tracker->last_data.timecode == 0) {
+	if (!tracker->is_initialized) {
 		tracker->pose.Rot[0] = 1.;
 		if (tracker->last_data.datamask == imu_calibration_iterations) {
 			tracker->last_data = *data;
@@ -182,7 +182,7 @@ void survive_imu_tracker_integrate(SurviveObject *so, SurviveIMUTracker *tracker
 			const FLT up[3] = {0, 0, 1};
 			quatfrom2vectors(tracker->pose.Rot, tracker->updir, up);
 			tracker->accel_scale_bias = 1. / magnitude3d(tracker->updir);
-
+			tracker->is_initialized = true;
 			return;
 		}
 
@@ -233,6 +233,11 @@ void survive_imu_tracker_integrate(SurviveObject *so, SurviveIMUTracker *tracker
 
 void survive_imu_tracker_integrate_observation(SurviveObject *so, uint32_t timecode, SurviveIMUTracker *tracker,
 											   SurvivePose *pose, const FLT *R) {
+	if (!tracker->is_initialized) {
+		tracker->pose = *pose;
+		return;
+	}
+		
 	// Kalman filter assuming:
 	// F -> Identity
 	// H -> Identity