File content as of revision 3:5eb51c7b0ca3:

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