Diff: FastECompass.cpp

Revision:

2:c14ec86128a6

Parent:

0:a3affe6b4fe8

--- a/FastECompass.cpp	Sat Jun 18 21:26:52 2016 +0000
+++ b/FastECompass.cpp	Mon Jun 20 15:26:07 2016 +0000
@@ -55,17 +55,15 @@
 	accsensor->getAxis(acc_raw);
 	magsensor->getAxis(mag_raw);
 
-	mag_raw.x=mag_raw.x-i16magcal.itrVx;
-	mag_raw.y=mag_raw.y-i16magcal.itrVy;
-	mag_raw.z=mag_raw.z-i16magcal.itrVz;
-
-	//printf("%d,%d,%d\n",mag_raw.x,mag_raw.y,mag_raw.z);
-	//printf("%d,%d,%d\n",acc_raw.x,acc_raw.y,acc_raw.z);
+	mag_raw.x-=i16magcal.itrVx;
+	mag_raw.y-=i16magcal.itrVy;
+	mag_raw.z-=i16magcal.itrVz;
 
 	phi=atan2_q15(acc_raw.z,acc_raw.y);
 
-	roll=int32_t(phi);
-
+	roll=(RAD_Q15_TODEG(int32_t(phi)))>>QFORMAT;
+	roll=(roll>90)?(180-roll):((roll<-90)?(-180-roll):roll);
+	
 	phi=(phi<<QFORMAT)/PIx2_Q15;
 
 	if(phi<0){
@@ -79,7 +77,9 @@
 
 	theta=atan2_q15(((((isin_phi*acc_raw.y)+(icos_phi*acc_raw.z)))>>15),-acc_raw.x);
 
-	pitch=(int32_t)theta;
+	pitch=(RAD_Q15_TODEG((int32_t)theta))>>QFORMAT;
+
+	pitch=(pitch>90)?(180-pitch):((pitch<-90)?(-180-pitch):pitch);
 
 	theta=(theta<<QFORMAT)/PIx2_Q15;
 
@@ -97,37 +97,24 @@
 	bfx=((mag_raw.x*icos_theta)+(((mag_raw.y*isin_theta)>>QFORMAT)*isin_phi)+(mag_raw.z*((isin_theta*icos_phi)>>QFORMAT)))>>QFORMAT;
 
 	psi=atan2_q15(bfx,-bfy);
-
-	yaw=(int32_t)psi;
-
+	
+	yaw=(RAD_Q15_TODEG((int32_t)psi))>>QFORMAT;
+	yaw=(yaw<0)?(yaw+360):yaw;
+	
 }
 
 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;
 }