Federico Luis Pinna Gonzalez / FastECompass

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

FastECompass.cpp@3:5eb51c7b0ca3, 2016-06-20 (annotated)

Committer:
Cotzo
Date:
Mon Jun 20 17:53:52 2016 +0000
Revision:
3:5eb51c7b0ca3
Parent:
0:a3affe6b4fe8 
Bug fixed in the metodo updateSensors

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Cotzo 0:a3affe6b4fe8 1 /*
Cotzo 0:a3affe6b4fe8 2 * FastECompass Library
Cotzo 0:a3affe6b4fe8 3 * Autor: Federico Pinna
Cotzo 0:a3affe6b4fe8 4 * Date: 29 de may.de 2016
Cotzo 0:a3affe6b4fe8 5 */
Cotzo 0:a3affe6b4fe8 6 #include "FastECompass.h"
Cotzo 0:a3affe6b4fe8 7
Cotzo 0:a3affe6b4fe8 8
Cotzo 0:a3affe6b4fe8 9 FastECompass::FastECompass(MotionSensor *magsensor, MotionSensor *accsensor){
Cotzo 0:a3affe6b4fe8 10
Cotzo 0:a3affe6b4fe8 11 roll=pitch=yaw=0;
Cotzo 0:a3affe6b4fe8 12 this->magsensor = magsensor;
Cotzo 0:a3affe6b4fe8 13 this->accsensor = accsensor;
Cotzo 0:a3affe6b4fe8 14
Cotzo 0:a3affe6b4fe8 15 }
Cotzo 0:a3affe6b4fe8 16
Cotzo 0:a3affe6b4fe8 17 void FastECompass::calibrateSensors(){
Cotzo 0:a3affe6b4fe8 18
Cotzo 0:a3affe6b4fe8 19 int16_t i;
Cotzo 0:a3affe6b4fe8 20
Cotzo 0:a3affe6b4fe8 21
Cotzo 0:a3affe6b4fe8 22 for(i=0; i<MAGBUFFSIZE; i++) {
Cotzo 0:a3affe6b4fe8 23
Cotzo 0:a3affe6b4fe8 24 magsensor->getAxis(mag_raw);
Cotzo 0:a3affe6b4fe8 25 magbuf.iBx[i] = mag_raw.x;
Cotzo 0:a3affe6b4fe8 26 magbuf.iBy[i] = mag_raw.y;
Cotzo 0:a3affe6b4fe8 27 magbuf.iBz[i] = mag_raw.z;
Cotzo 0:a3affe6b4fe8 28 wait(0.04);
Cotzo 0:a3affe6b4fe8 29
Cotzo 0:a3affe6b4fe8 30 };
Cotzo 0:a3affe6b4fe8 31
Cotzo 0:a3affe6b4fe8 32 ResetMagCalibration(&magcal);
Cotzo 0:a3affe6b4fe8 33 magUpdateCalibration(&magcal,&magbuf);
Cotzo 0:a3affe6b4fe8 34
Cotzo 0:a3affe6b4fe8 35 i16magcal.itrVx = (int16_t)magcal.ftrVx*10.f;
Cotzo 0:a3affe6b4fe8 36 i16magcal.itrVy = (int16_t)magcal.ftrVy*10.f;
Cotzo 0:a3affe6b4fe8 37 i16magcal.itrVz = (int16_t)magcal.ftrVz*10.f;
Cotzo 0:a3affe6b4fe8 38
Cotzo 0:a3affe6b4fe8 39
Cotzo 0:a3affe6b4fe8 40 }
Cotzo 0:a3affe6b4fe8 41
Cotzo 0:a3affe6b4fe8 42 void FastECompass::enableSensors(){
Cotzo 0:a3affe6b4fe8 43 accsensor->enable();
Cotzo 0:a3affe6b4fe8 44 magsensor->enable();
Cotzo 0:a3affe6b4fe8 45
Cotzo 0:a3affe6b4fe8 46
Cotzo 0:a3affe6b4fe8 47 }
Cotzo 0:a3affe6b4fe8 48 void FastECompass::updateSensors(){
Cotzo 0:a3affe6b4fe8 49
Cotzo 0:a3affe6b4fe8 50 int64_t phi,theta,psi;
Cotzo 0:a3affe6b4fe8 51 int32_t isin_phi,icos_phi;
Cotzo 0:a3affe6b4fe8 52 int32_t isin_theta,icos_theta;
Cotzo 0:a3affe6b4fe8 53 int32_t bfy,bfx;
Cotzo 0:a3affe6b4fe8 54
Cotzo 0:a3affe6b4fe8 55 accsensor->getAxis(acc_raw);
Cotzo 0:a3affe6b4fe8 56 magsensor->getAxis(mag_raw);
Cotzo 0:a3affe6b4fe8 57
Cotzo 0:a3affe6b4fe8 58 mag_raw.x=mag_raw.x-i16magcal.itrVx;
Cotzo 0:a3affe6b4fe8 59 mag_raw.y=mag_raw.y-i16magcal.itrVy;
Cotzo 0:a3affe6b4fe8 60 mag_raw.z=mag_raw.z-i16magcal.itrVz;
Cotzo 0:a3affe6b4fe8 61
Cotzo 0:a3affe6b4fe8 62 //printf("%d,%d,%d\n",mag_raw.x,mag_raw.y,mag_raw.z);
Cotzo 0:a3affe6b4fe8 63 //printf("%d,%d,%d\n",acc_raw.x,acc_raw.y,acc_raw.z);
Cotzo 0:a3affe6b4fe8 64
Cotzo 0:a3affe6b4fe8 65 phi=atan2_q15(acc_raw.z,acc_raw.y);
Cotzo 0:a3affe6b4fe8 66
Cotzo 0:a3affe6b4fe8 67 roll=int32_t(phi);
Cotzo 0:a3affe6b4fe8 68
Cotzo 3:5eb51c7b0ca3 69 roll=(RAD_Q15_TODEG(roll))>>QFORMAT;
Cotzo 3:5eb51c7b0ca3 70
Cotzo 3:5eb51c7b0ca3 71 if(roll>90) roll=180-roll;
Cotzo 3:5eb51c7b0ca3 72 if(roll<-90) roll=-180-roll;
Cotzo 3:5eb51c7b0ca3 73
Cotzo 3:5eb51c7b0ca3 74
Cotzo 3:5eb51c7b0ca3 75
Cotzo 0:a3affe6b4fe8 76 phi=(phi<<QFORMAT)/PIx2_Q15;
Cotzo 0:a3affe6b4fe8 77
Cotzo 0:a3affe6b4fe8 78 if(phi<0){
Cotzo 0:a3affe6b4fe8 79 phi=-phi;
Cotzo 0:a3affe6b4fe8 80 isin_phi=-arm_sin_q15(phi);
Cotzo 0:a3affe6b4fe8 81 icos_phi=arm_cos_q15(phi);
Cotzo 0:a3affe6b4fe8 82 }else{
Cotzo 0:a3affe6b4fe8 83 isin_phi=arm_sin_q15(phi);
Cotzo 0:a3affe6b4fe8 84 icos_phi=arm_cos_q15(phi);
Cotzo 0:a3affe6b4fe8 85 }
Cotzo 0:a3affe6b4fe8 86
Cotzo 0:a3affe6b4fe8 87 theta=atan2_q15(((((isin_phi*acc_raw.y)+(icos_phi*acc_raw.z)))>>15),-acc_raw.x);
Cotzo 0:a3affe6b4fe8 88
Cotzo 0:a3affe6b4fe8 89 pitch=(int32_t)theta;
Cotzo 0:a3affe6b4fe8 90
Cotzo 3:5eb51c7b0ca3 91
Cotzo 3:5eb51c7b0ca3 92 pitch=(RAD_Q15_TODEG(pitch))>>QFORMAT;
Cotzo 3:5eb51c7b0ca3 93
Cotzo 3:5eb51c7b0ca3 94 if(pitch>90)pitch=180-pitch;
Cotzo 3:5eb51c7b0ca3 95 if(pitch<-90)pitch=-180-pitch;
Cotzo 3:5eb51c7b0ca3 96
Cotzo 3:5eb51c7b0ca3 97
Cotzo 0:a3affe6b4fe8 98 theta=(theta<<QFORMAT)/PIx2_Q15;
Cotzo 0:a3affe6b4fe8 99
Cotzo 0:a3affe6b4fe8 100 if(theta<0){
Cotzo 0:a3affe6b4fe8 101 theta=-theta;
Cotzo 0:a3affe6b4fe8 102 isin_theta=-arm_sin_q15(theta);
Cotzo 0:a3affe6b4fe8 103 icos_theta=arm_cos_q15(theta);
Cotzo 0:a3affe6b4fe8 104 }else{
Cotzo 0:a3affe6b4fe8 105 isin_theta=arm_sin_q15(theta);
Cotzo 0:a3affe6b4fe8 106 icos_theta=arm_cos_q15(theta);
Cotzo 0:a3affe6b4fe8 107 }
Cotzo 0:a3affe6b4fe8 108
Cotzo 0:a3affe6b4fe8 109
Cotzo 0:a3affe6b4fe8 110 bfy=(((mag_raw.z*isin_phi)-(mag_raw.y*icos_phi))>>QFORMAT);
Cotzo 0:a3affe6b4fe8 111 bfx=((mag_raw.x*icos_theta)+(((mag_raw.y*isin_theta)>>QFORMAT)*isin_phi)+(mag_raw.z*((isin_theta*icos_phi)>>QFORMAT)))>>QFORMAT;
Cotzo 0:a3affe6b4fe8 112
Cotzo 0:a3affe6b4fe8 113 psi=atan2_q15(bfx,-bfy);
Cotzo 0:a3affe6b4fe8 114
Cotzo 0:a3affe6b4fe8 115 yaw=(int32_t)psi;
Cotzo 3:5eb51c7b0ca3 116
Cotzo 3:5eb51c7b0ca3 117 yaw=(RAD_Q15_TODEG(yaw))>>QFORMAT;
Cotzo 3:5eb51c7b0ca3 118 if(yaw<0)yaw+=360;
Cotzo 3:5eb51c7b0ca3 119
Cotzo 0:a3affe6b4fe8 120
Cotzo 0:a3affe6b4fe8 121 }
Cotzo 0:a3affe6b4fe8 122
Cotzo 0:a3affe6b4fe8 123 int32_t FastECompass::getRoll(){
Cotzo 3:5eb51c7b0ca3 124
Cotzo 0:a3affe6b4fe8 125 return roll;
Cotzo 0:a3affe6b4fe8 126 }
Cotzo 0:a3affe6b4fe8 127
Cotzo 0:a3affe6b4fe8 128 int32_t FastECompass::getPitch(){
Cotzo 3:5eb51c7b0ca3 129
Cotzo 0:a3affe6b4fe8 130 return pitch;
Cotzo 0:a3affe6b4fe8 131 }
Cotzo 0:a3affe6b4fe8 132
Cotzo 0:a3affe6b4fe8 133 int32_t FastECompass::getYaw(){
Cotzo 3:5eb51c7b0ca3 134
Cotzo 0:a3affe6b4fe8 135 return yaw;
Cotzo 0:a3affe6b4fe8 136 }
Cotzo 0:a3affe6b4fe8 137
Cotzo 0:a3affe6b4fe8 138
Cotzo 0:a3affe6b4fe8 139
Cotzo 0:a3affe6b4fe8 140