Federico Luis Pinna Gonzalez / FastECompass

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   CommonTables FastAtan2 FastMathFunctions Magnetic

Diff: FastECompass.cpp

Revision:
3:5eb51c7b0ca3
Parent:
0:a3affe6b4fe8
--- a/FastECompass.cpp	Sat Jun 18 21:26:52 2016 +0000
+++ b/FastECompass.cpp	Mon Jun 20 17:53:52 2016 +0000
@@ -66,6 +66,13 @@
 
 	roll=int32_t(phi);
 
+	roll=(RAD_Q15_TODEG(roll))>>QFORMAT;
+
+	if(roll>90) roll=180-roll;
+	if(roll<-90) roll=-180-roll;
+
+
+
 	phi=(phi<<QFORMAT)/PIx2_Q15;
 
 	if(phi<0){
@@ -81,6 +88,13 @@
 
 	pitch=(int32_t)theta;
 
+	
+	pitch=(RAD_Q15_TODEG(pitch))>>QFORMAT;
+
+	if(pitch>90)pitch=180-pitch;
+	if(pitch<-90)pitch=-180-pitch;
+
+
 	theta=(theta<<QFORMAT)/PIx2_Q15;
 
 	if(theta<0){
@@ -99,35 +113,25 @@
 	psi=atan2_q15(bfx,-bfy);
 
 	yaw=(int32_t)psi;
+	
+	yaw=(RAD_Q15_TODEG(yaw))>>QFORMAT;
+	if(yaw<0)yaw+=360;
+	
 
 }
 
 int32_t FastECompass::getRoll(){
-
-	roll=(RAD_Q15_TODEG(roll))>>QFORMAT;
-
-	if(roll>90) roll=180-roll;
-	if(roll<-90) roll=-180-roll;
-
+	
 	return roll;
 }
 
 int32_t FastECompass::getPitch(){
-
-
-	pitch=(RAD_Q15_TODEG(pitch))>>QFORMAT;
-
-	if(pitch>90)pitch=180-pitch;
-	if(pitch<-90)pitch=-180-pitch;
-
+	
 	return pitch;
 }
 
 int32_t FastECompass::getYaw(){
-
-
-	yaw=(RAD_Q15_TODEG(yaw))>>QFORMAT;
-
+	
 	return yaw;
 }