Revision 3:5eb51c7b0ca3, committed 2016-06-20

Comitter:

Cotzo

Date:

Mon Jun 20 17:53:52 2016 +0000

Parent:

1:07a66572d787

Commit message:

Bug fixed in the metodo updateSensors

Changed in this revision

--- 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;
 }
 

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/FastECompass.cpp.orig	Mon Jun 20 17:53:52 2016 +0000
@@ -0,0 +1,123 @@
+/*
+*   FastECompass Library
+*   Autor: Federico Pinna
+*   Date: 29 de may.de 2016
+*/
+#include "FastECompass.h"
+
+
+FastECompass::FastECompass(MotionSensor *magsensor, MotionSensor *accsensor){
+
+	roll=pitch=yaw=0;
+	this->magsensor = magsensor;
+	this->accsensor = accsensor;
+
+}
+
+void FastECompass::calibrateSensors(){
+
+	int16_t i;
+	
+
+	for(i=0; i<MAGBUFFSIZE; i++) {
+
+			magsensor->getAxis(mag_raw);
+			magbuf.iBx[i] = mag_raw.x;
+			magbuf.iBy[i] = mag_raw.y;
+			magbuf.iBz[i] = mag_raw.z;
+			wait(0.04);
+
+	};
+
+	ResetMagCalibration(&magcal);
+	magUpdateCalibration(&magcal,&magbuf);
+
+	i16magcal.itrVx = (int16_t)magcal.ftrVx*10.f;
+	i16magcal.itrVy = (int16_t)magcal.ftrVy*10.f;
+	i16magcal.itrVz = (int16_t)magcal.ftrVz*10.f;
+
+
+}
+
+void FastECompass::enableSensors(){
+	accsensor->enable();
+	magsensor->enable();
+
+
+}
+void FastECompass::updateSensors(){
+
+	int64_t phi,theta,psi;
+	int32_t isin_phi,icos_phi;
+	int32_t isin_theta,icos_theta;
+	int32_t bfy,bfx;
+
+	accsensor->getAxis(acc_raw);
+	magsensor->getAxis(mag_raw);
+
+	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=(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){
+		phi=-phi;
+		isin_phi=-arm_sin_q15(phi);
+		icos_phi=arm_cos_q15(phi);
+	}else{
+		isin_phi=arm_sin_q15(phi);
+		icos_phi=arm_cos_q15(phi);
+	}
+
+	theta=atan2_q15(((((isin_phi*acc_raw.y)+(icos_phi*acc_raw.z)))>>15),-acc_raw.x);
+
+	pitch=(RAD_Q15_TODEG((int32_t)theta))>>QFORMAT;
+
+	pitch=(pitch>90)?(180-pitch):((pitch<-90)?(-180-pitch):pitch);
+
+	theta=(theta<<QFORMAT)/PIx2_Q15;
+
+	if(theta<0){
+		theta=-theta;
+		isin_theta=-arm_sin_q15(theta);
+		icos_theta=arm_cos_q15(theta);
+	}else{
+		isin_theta=arm_sin_q15(theta);
+		icos_theta=arm_cos_q15(theta);
+	}
+
+
+	bfy=(((mag_raw.z*isin_phi)-(mag_raw.y*icos_phi))>>QFORMAT);
+	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=(RAD_Q15_TODEG((int32_t)psi))>>QFORMAT;
+	yaw=(yaw<0)?(yaw+360):yaw;
+	
+}
+
+int32_t FastECompass::getRoll(){
+	
+	return this->roll;
+}
+
+int32_t FastECompass::getPitch(){
+	
+	return this->pitch;
+}
+
+int32_t FastECompass::getYaw(){
+
+	return this->yaw;
+}
+
+
+
+

--- a/FastECompass.h	Sat Jun 18 21:26:52 2016 +0000
+++ b/FastECompass.h	Mon Jun 20 17:53:52 2016 +0000
@@ -83,15 +83,15 @@
 	*/
 	void updateSensors();
 	/** Gets the value of the roll angle
-	* @return An integer representing the roll angle in the range of -90 to 90.
+	* @return An integer representing the roll angle in deg the range of -90 to 90.
 	*/
 	int32_t getRoll();
 	/** Gets the value of the pitch angle
-	* @return An integer representing the pitch angle in the range of -90 to 90.
+	* @return An integer representing the pitch angle in deg the range of -90 to 90.
 	*/
 	int32_t getPitch();
 	/** Gets the value of the yaw angle
-	* @return An integer representing the yaw angle in the range of -180 to 180.
+	* @return An integer representing the yaw angle in deg in the range of 0 to 360.
 	*/
 	int32_t getYaw();