Diff: FastECompass.cpp

Revision:

0:a3affe6b4fe8

Child:

2:c14ec86128a6

Child:

3:5eb51c7b0ca3

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/FastECompass.cpp	Sat Jun 18 21:16:34 2016 +0000
@@ -0,0 +1,136 @@
+/*
+*   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=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);
+
+	phi=atan2_q15(acc_raw.z,acc_raw.y);
+
+	roll=int32_t(phi);
+
+	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=(int32_t)theta;
+
+	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=(int32_t)psi;
+
+}
+
+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;
+}
+
+
+
+