Fixed acceleration only fallback, some cleanup here and there

src/drv_android/android.c

@@ -30,8 +30,60 @@ typedef struct {
     #endif
 } android_priv;
 
+static void nofusion_update(fusion* me, float dt, const vec3f* accel)
+{
+	//avg raw accel data to smooth jitter, and normalise
+	ofq_add(&me->accel_fq, accel);
+	vec3f accel_mean;
+	ofq_get_mean(&me->accel_fq, &accel_mean);
+	vec3f acc_n = accel_mean;
+	ovec3f_normalize_me(&acc_n);
+
+
+	//reference vectors for axis-angle
+	vec3f xyzv[3] = {
+		{1,0,0},
+		{0,1,0},
+		{0,0,1}
+	};
+	quatf roll, pitch;
+
+	//pitch is rot around x, based on gravity in z and y axes
+	oquatf_init_axis(&pitch, xyzv+0, atan2f(-acc_n.z, -acc_n.y));
+
+	//roll is rot around z, based on gravity in x and y axes
+	//note we need to invert the values when the device is upside down (y < 0) for proper results
+	oquatf_init_axis(&roll, xyzv+2, acc_n.y < 0 ? atan2f(-acc_n.x, -acc_n.y) : atan2f(acc_n.x, acc_n.y));
+
+
+
+	quatf or = {0,0,0,1};
+	//order of applying is yaw-pitch-roll
+	//yaw is not possible using only accel
+	oquatf_mult_me(&or, &pitch);
+	oquatf_mult_me(&or, &roll);
+
+	me->orient = or;
+}
+
+//shorter buffers for frame smoothing
+static void nofusion_init(fusion* me)
+{
+	memset(me, 0, sizeof(fusion));
+	me->orient.w = 1.0f;
+
+	ofq_init(&me->mag_fq, 10);
+	ofq_init(&me->accel_fq, 10);
+	ofq_init(&me->ang_vel_fq, 10);
+
+	me->flags = FF_USE_GRAVITY;
+	me->grav_gain = 0.05f;
+}
+
+//Static variable for timeDelta;
 static float timestamp;
 
+//Android callback for the sensor event queue
 static int android_sensor_callback(int fd, int events, void* data)
 {
     android_priv* priv = (android_priv*)data;
@@ -69,7 +121,12 @@ static int android_sensor_callback(int fd, int events, void* data)
             if (timestamp != 0)
                 dT= (lastevent_timestamp - timestamp) * (1.0f / 1000000000.0f);
 
-            ofusion_update(&priv->sensor_fusion, dT, &gyro, &accel, &mag);
+            //Check if accelerometer only fallback is required
+            if (!priv->gyroscopeSensor)
+                nofusion_update(&priv->sensor_fusion, dT, &accel);
+            else
+                ofusion_update(&priv->sensor_fusion, dT, &gyro, &accel, &mag); //default
+
             timestamp = lastevent_timestamp;
     }
     return 1;
@@ -105,57 +162,6 @@ static void update_device(ohmd_device* device)
     }
 }
 
-static void nofusion_update(fusion* me, float dt, const vec3f* accel)
-{
-	//avg raw accel data to smooth jitter, and normalise
-	ofq_add(&me->accel_fq, accel);
-	vec3f accel_mean;
-	ofq_get_mean(&me->accel_fq, &accel_mean);
-	vec3f acc_n = accel_mean;
-	ovec3f_normalize_me(&acc_n);
-
-
-	//reference vectors for axis-angle
-	vec3f xyzv[3] = {
-		{1,0,0},
-		{0,1,0},
-		{0,0,1}
-	};
-	quatf roll, pitch;
-
-	//pitch is rot around x, based on gravity in z and y axes
-	oquatf_init_axis(&pitch, xyzv+0, atan2f(-acc_n.z, -acc_n.y));
-
-	//roll is rot around z, based on gravity in x and y axes
-	//note we need to invert the values when the device is upside down (y < 0) for proper results
-	oquatf_init_axis(&roll, xyzv+2, acc_n.y < 0 ? atan2f(-acc_n.x, -acc_n.y) : atan2f(acc_n.x, acc_n.y));
-
-
-
-	quatf or = {0,0,0,1};
-	//order of applying is yaw-pitch-roll
-	//yaw is not possible using only accel
-	oquatf_mult_me(&or, &pitch);
-	oquatf_mult_me(&or, &roll);
-
-	me->orient = or;
-}
-
-//shorter buffers for frame smoothing
-void nofusion_init(fusion* me)
-{
-	memset(me, 0, sizeof(fusion));
-	me->orient.w = 1.0f;
-
-	ofq_init(&me->mag_fq, 10);
-	ofq_init(&me->accel_fq, 10);
-	ofq_init(&me->ang_vel_fq, 10);
-
-	me->flags = FF_USE_GRAVITY;
-	me->grav_gain = 0.05f;
-}
-
-
 static int getf(ohmd_device* device, ohmd_float_value type, float* out)
 {
 	android_priv* priv = (android_priv*)device;
@@ -248,11 +254,11 @@ static ohmd_device* open_device(ohmd_driver* driver, ohmd_device_desc* desc)
 
     priv->firstRun = 1; //need this since ASensorManager_createEventQueue requires a set android_app*
 
-    ///TODO: Use ASensorManager_getSensorList to set accel only fallback
-    //if (!priv->gyroscopeSensor)
-        //set accel only
-
-	ofusion_init(&priv->sensor_fusion);
+    //Check if accelerometer only fallback is required
+    if (!priv->gyroscopeSensor)
+        nofusion_init(&priv->sensor_fusion);
+    else
+        ofusion_init(&priv->sensor_fusion); //Default when all sensors are available
 
 	return (ohmd_device*)priv;
 }