File content as of revision 0:a3affe6b4fe8:

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