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
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();