From 1cbe268abcd11fae9a649468158080bed2bbfdb8 Mon Sep 17 00:00:00 2001
From: "matthias.ringwald"
 <matthias.ringwald@1a0a8af8-31b5-11de-8e0c-53a27eea117e>
Date: Fri, 6 Nov 2009 21:19:35 +0000
Subject: [PATCH] try again with WiiMote accel value. Pitch working correctly

---
 TODO.txt                                      |  9 ++++-
 .../Classes/WiiMoteOpenGLDemoAppDelegate.m    | 37 ++++++++++++++++---
 2 files changed, 39 insertions(+), 7 deletions(-)

diff --git a/TODO.txt b/TODO.txt
index 5185bbd7b..2c11dd1f9 100644
--- a/TODO.txt
+++ b/TODO.txt
@@ -2,11 +2,18 @@
 
 NEXT:
 - WiiMote Demo
-  - upside down is not shown correctly
+  - implement roll
+  - video for YouTube
+
 - check/fix BTstack for iPhone 2G
+
 - figure out how to add BTstack logo to Cydia repository list
 - sign BTstack repository
 
+- add news coverage on wiki
+- release notes (features and limits)
+- install instructions with Cydia usage screenshots
+
 == Release Version 0.1
 
 - figure out how to receive iPhone System Power IONotifications (in BTdaemon) to detect,
diff --git a/example/WiiMoteOpenGLDemo/Classes/WiiMoteOpenGLDemoAppDelegate.m b/example/WiiMoteOpenGLDemo/Classes/WiiMoteOpenGLDemoAppDelegate.m
index f3dce3d16..4d6794de6 100644
--- a/example/WiiMoteOpenGLDemo/Classes/WiiMoteOpenGLDemoAppDelegate.m
+++ b/example/WiiMoteOpenGLDemo/Classes/WiiMoteOpenGLDemoAppDelegate.m
@@ -67,16 +67,17 @@ static void bt_data_cb(uint8_t x, uint8_t y, uint8_t z){
 	float az = z - 128;
 	
 	// mini calib
-	az /= 2;
+	// az /= 2;
 	
-#if 1
+#if 0
 	// normalize vector
 	float length = sqrt( ax*ax + ay*ay + az*az);
 	ax /= length;
 	ay /= length;
 	az /= length;
 #endif
-	
+
+#if 0
 	// cross product between A and (0,0,1) 
 	float crossX = ay;
 	float crossY = -ax;
@@ -94,7 +95,7 @@ static void bt_data_cb(uint8_t x, uint8_t y, uint8_t z){
 	int pitch = atan2( 2*(omega*crossX), omega*omega - crossX*crossX - crossY*crossY)* 180 / M_PI;
 	int roll  = atan2( 2*(crossX * crossY), omega*omega + crossX*crossX - crossY*crossY) * 180 / M_PI;
 	int theta = 0;
-	
+#endif	
 #if 0	
 	int roll  = atan2(ax, sqrt(ay*ay+az*az)) * 180 / M_PI; 
 	int pitch = atan2(ay, sqrt(ax*ax+az*az)) * 180 / M_PI;
@@ -104,16 +105,37 @@ static void bt_data_cb(uint8_t x, uint8_t y, uint8_t z){
 	}
 #endif
 	
-	if ( (++counter & 15) == 0)
+	// sort axes
+	float h = az;
+	az = -ay;
+	ay = h;
+
+	// calc
+	int roll =  0;
+	int pitch = atan2(ay, az) * 180 / M_PI;
+	int theta = 0;
+	
+	// if ( (++counter & 15) == 0)
 		NSLog(@"BT data: %f %f %f: pitch %i, roll %i, yaw %i", ax , ay ,az, pitch, roll, theta);
 
 #if 1
+	static int lastPitch;
+
+	if (abs(lastPitch - pitch) > 180) {
+		if (pitch > lastPitch) {
+			pitch -= 360;
+		} else {
+			pitch += 360;
+		}
+	}
+	
 	// moving average of size SIZE
 	static int pos = 0;
 	static int historyRoll[SIZE];
 	static int historyPitch[SIZE];
 	static int historyTheta[SIZE];
 	
+	
 	historyRoll[pos] = roll;
 	historyPitch[pos] = pitch;
 	historyTheta[pos] = theta;
@@ -130,8 +152,11 @@ static void bt_data_cb(uint8_t x, uint8_t y, uint8_t z){
 	roll = roll / SIZE;
 	pitch = pitch / SIZE;
 	theta = theta / SIZE;
+	
+	lastPitch = pitch;
 #endif	
-	[[theMainApp glView] setRotationX:-pitch Y:roll Z:0];
+	// hack 
+	[[theMainApp glView] setRotationX:(-pitch+90) Y:roll Z:0];
 }
 
 void packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){