Kiko Ishimoto
MPU9250
Fork of
MPU9250_SPI
by 
Mu kylong
Diff: MPU9250.cpp
- Revision:
- 12:a70c193d8195
- Parent:
- 11:084e8ba240c1
--- a/MPU9250.cpp Tue Jul 01 13:59:45 2014 +0000
+++ b/MPU9250.cpp Mon May 09 05:45:18 2016 +0000
@@ -5,7 +5,18 @@
#include <mbed.h>
#include "MPU9250.h"
-mpu9250_spi::mpu9250_spi(SPI& _spi, PinName _cs) : spi(_spi), cs(_cs) {}
+mpu9250_spi::mpu9250_spi(SPI& _spi, PinName _cs) : spi(_spi), cs(_cs)
+{
+ /* uint8_t offset[3][2] =
+ {-21 , MPUREG_XG_OFFS_TC};
+ for(i=0; i<offset; i++) {
+ WriteReg(offset[i][1], offset[i][0]);
+ wait(0.001); //I2C must slow down the write speed, otherwise it won't work
+ }*/
+ Magnetometer_offset[0]=0;
+ Magnetometer_offset[1]=0;
+ Magnetometer_offset[2]=0;
+}
unsigned int mpu9250_spi::WriteReg( uint8_t WriteAddr, uint8_t WriteData )
{
@@ -42,14 +53,15 @@
BITS_DLPF_CFG_98HZ
BITS_DLPF_CFG_42HZ
BITS_DLPF_CFG_20HZ
-BITS_DLPF_CFG_10HZ
-BITS_DLPF_CFG_5HZ
+BITS_DLPF_CFG_10HZ
+BITS_DLPF_CFG_5HZ
BITS_DLPF_CFG_2100HZ_NOLPF
returns 1 if an error occurred
-----------------------------------------------------------------------------------------------*/
#define MPU_InitRegNum 17
-bool mpu9250_spi::init(int sample_rate_div,int low_pass_filter){
+bool mpu9250_spi::init(int sample_rate_div,int low_pass_filter)
+{
uint8_t i = 0;
uint8_t MPU_Init_Data[MPU_InitRegNum][2] = {
{0x80, MPUREG_PWR_MGMT_1}, // Reset Device
@@ -64,7 +76,7 @@
//{0x20, MPUREG_USER_CTRL}, // Enable AUX
{0x20, MPUREG_USER_CTRL}, // I2C Master mode
{0x0D, MPUREG_I2C_MST_CTRL}, // I2C configuration multi-master IIC 400KHz
-
+
{AK8963_I2C_ADDR, MPUREG_I2C_SLV0_ADDR}, //Set the I2C slave addres of AK8963 and set for write.
//{0x09, MPUREG_I2C_SLV4_CTRL},
//{0x81, MPUREG_I2C_MST_DELAY_CTRL}, //Enable I2C delay
@@ -76,7 +88,7 @@
{AK8963_CNTL1, MPUREG_I2C_SLV0_REG}, //I2C slave 0 register address from where to begin data transfer
{0x12, MPUREG_I2C_SLV0_DO}, // Register value to continuous measurement in 16bit
{0x81, MPUREG_I2C_SLV0_CTRL} //Enable I2C and set 1 byte
-
+
};
spi.format(8,0);
spi.frequency(1000000);
@@ -88,7 +100,7 @@
set_acc_scale(2);
set_gyro_scale(250);
-
+
//AK8963_calib_Magnetometer(); //Can't load this function here , strange problem?
return 0;
}
@@ -102,39 +114,40 @@
BITS_FS_16G
returns the range set (2,4,8 or 16)
-----------------------------------------------------------------------------------------------*/
-unsigned int mpu9250_spi::set_acc_scale(int scale){
+unsigned int mpu9250_spi::set_acc_scale(int scale)
+{
unsigned int temp_scale;
WriteReg(MPUREG_ACCEL_CONFIG, scale);
-
- switch (scale){
+
+ switch (scale) {
case BITS_FS_2G:
acc_divider=16384;
- break;
+ break;
case BITS_FS_4G:
acc_divider=8192;
- break;
+ break;
case BITS_FS_8G:
acc_divider=4096;
- break;
+ break;
case BITS_FS_16G:
acc_divider=2048;
- break;
+ break;
}
temp_scale=WriteReg(MPUREG_ACCEL_CONFIG|READ_FLAG, 0x00);
-
- switch (temp_scale){
+
+ switch (temp_scale) {
case BITS_FS_2G:
temp_scale=2;
- break;
+ break;
case BITS_FS_4G:
temp_scale=4;
- break;
+ break;
case BITS_FS_8G:
temp_scale=8;
- break;
+ break;
case BITS_FS_16G:
temp_scale=16;
- break;
+ break;
}
return temp_scale;
}
@@ -150,37 +163,38 @@
BITS_FS_2000DPS
returns the range set (250,500,1000 or 2000)
-----------------------------------------------------------------------------------------------*/
-unsigned int mpu9250_spi::set_gyro_scale(int scale){
+unsigned int mpu9250_spi::set_gyro_scale(int scale)
+{
unsigned int temp_scale;
WriteReg(MPUREG_GYRO_CONFIG, scale);
- switch (scale){
+ switch (scale) {
case BITS_FS_250DPS:
gyro_divider=131;
- break;
+ break;
case BITS_FS_500DPS:
gyro_divider=65.5;
- break;
+ break;
case BITS_FS_1000DPS:
gyro_divider=32.8;
- break;
+ break;
case BITS_FS_2000DPS:
gyro_divider=16.4;
- break;
+ break;
}
temp_scale=WriteReg(MPUREG_GYRO_CONFIG|READ_FLAG, 0x00);
- switch (temp_scale){
+ switch (temp_scale) {
case BITS_FS_250DPS:
temp_scale=250;
- break;
+ break;
case BITS_FS_500DPS:
temp_scale=500;
- break;
+ break;
case BITS_FS_1000DPS:
temp_scale=1000;
- break;
+ break;
case BITS_FS_2000DPS:
temp_scale=2000;
- break;
+ break;
}
return temp_scale;
}
@@ -192,7 +206,8 @@
mpu9250 which should be 104 when in SPI mode.
returns the I2C address (104)
-----------------------------------------------------------------------------------------------*/
-unsigned int mpu9250_spi::whoami(){
+unsigned int mpu9250_spi::whoami()
+{
unsigned int response;
response=WriteReg(MPUREG_WHOAMI|READ_FLAG, 0x00);
return response;
@@ -217,8 +232,10 @@
bit_data=((int16_t)response[i*2]<<8)|response[i*2+1];
data=(float)bit_data;
accelerometer_data[i]=data/acc_divider;
+ accelerometer_data[i]=0.1*accelerometer_data[i]+0.9*accelerometer_data_prev[i];
+ accelerometer_data_prev[i]=accelerometer_data[i];
}
-
+
}
/*-----------------------------------------------------------------------------------------------
@@ -245,10 +262,11 @@
/*-----------------------------------------------------------------------------------------------
READ TEMPERATURE
-usage: call this function to read temperature data.
+usage: call this function to read temperature data.
returns the value in °C
-----------------------------------------------------------------------------------------------*/
-void mpu9250_spi::read_temp(){
+void mpu9250_spi::read_temp()
+{
uint8_t response[2];
int16_t bit_data;
float data;
@@ -285,7 +303,8 @@
set_acc_scale(temp_scale);
}
-uint8_t mpu9250_spi::AK8963_whoami(){
+uint8_t mpu9250_spi::AK8963_whoami()
+{
uint8_t response;
WriteReg(MPUREG_I2C_SLV0_ADDR,AK8963_I2C_ADDR|READ_FLAG); //Set the I2C slave addres of AK8963 and set for read.
WriteReg(MPUREG_I2C_SLV0_REG, AK8963_WIA); //I2C slave 0 register address from where to begin data transfer
@@ -293,13 +312,76 @@
//WriteReg(MPUREG_I2C_SLV0_CTRL, 0x81); //Enable I2C and set bytes
wait(0.001);
- response=WriteReg(MPUREG_EXT_SENS_DATA_00|READ_FLAG, 0x00); //Read I2C
+ response=WriteReg(MPUREG_EXT_SENS_DATA_00|READ_FLAG, 0x00); //Read I2C
//ReadRegs(MPUREG_EXT_SENS_DATA_00,response,1);
- //response=WriteReg(MPUREG_I2C_SLV0_DO, 0x00); //Read I2C
+ //response=WriteReg(MPUREG_I2C_SLV0_DO, 0x00); //Read I2C
return response;
}
-void mpu9250_spi::AK8963_calib_Magnetometer(){
+double* mpu9250_spi::AK8936_read_Orientation(double *getdata)
+{
+
+ double gx = accelerometer_data[0];
+ double gy = accelerometer_data[1];
+ double gz = accelerometer_data[2];
+ double mx = Magnetometer[0];//-centorx;
+ double my = Magnetometer[1];//-centory;
+ double mz = Magnetometer[2];//-centorz;
+ double g0x=0.01,g0y=0.001,g0z=0.99;
+ double Gproduct = gx*g0x+gy*g0y+gz*g0z;
+ double Glengh = sqrt(gx*gx+gy*gy+gz*gz);
+ double G0lengh =sqrt(g0x*g0x+g0y*g0y+g0z*g0z);
+ double nx = (gy * g0z - gz * g0y);
+ double ny = (gz * g0x - gx * g0z);
+ double nz = (gx * g0y - gy * g0x);
+ double GPlengh = sqrt(nx*nx+ny*ny+nz*nz);
+ nx/=GPlengh;
+ ny/=GPlengh;
+ nz/=GPlengh;
+ double accang = acos((Gproduct)/(fabs(Glengh)*fabs(G0lengh)));/*
+ double hx=(nx*nx*(1-cos(accang))+cos(accang))*mx + (nx*ny*(1-cos(accang))+nz*sin(accang))*my + (nz*nx*(1-cos(accang))+ny*sin(accang))*mz;
+ double hy=(nx*ny*(1-cos(accang))+nz*sin(accang))*mx + (ny*ny*(1-cos(accang))+cos(accang))*my + (ny*nz*(1-cos(accang))+nx*sin(accang))*mz;*/
+ double Mrot[3][3] = {
+ {nx*nx*(1-cos(accang))+cos(accang) , nx*ny*(1-cos(accang))+nz*sin(accang) ,nz*nx*(1-cos(accang))+ny*sin(accang)},
+ {nx*ny*(1-cos(accang))+nz*sin(accang) , ny*ny*(1-cos(accang))+cos(accang) ,ny*nz*(1-cos(accang))+nx*sin(accang)},
+ {nz*nx*(1-cos(accang))+ny*sin(accang) , ny*nz*(1-cos(accang))+nx*sin(accang) ,nz*nz*(1-cos(accang))+cos(accang)}
+ };
+ double MrotD[3][3];
+ double temp[3][3];
+ for(int i=0; i<3; i++)
+ for(int j=0; j<3; j++) {
+ temp[j][i]=Mrot[i][j];
+ MrotD[j][i]=temp[j][i];
+ }
+ double hx = MrotD[0][0]*mx+MrotD[0][1]*my+MrotD[0][2]*mz;
+ double hy = MrotD[1][0]*mx+MrotD[1][1]*my+MrotD[1][2]*mz;
+ double hz = MrotD[2][0]*mx+MrotD[2][1]*my+MrotD[2][2]*mz;
+ double heading = atan2(hy,hx)*180/3.14;
+ if (heading > 0)
+ heading = 360 - heading;
+ else
+ heading = -heading;
+ float pitch = atan(-hx/sqrt(hy*hy+hz*hz))*180/3.14; //invert gx because +pitch is up. range is +/-90 degrees
+ float roll = atan(hy/sqrt(hx*hx+hz*hz))*180/3.14; // right side down is +roll
+ if (gz > 0)
+ {
+ if ( roll > 0)
+ roll = 180 - roll;
+ else
+ roll = -180 - roll;
+ }
+ getdata[0]=heading;
+ getdata[1]=pitch;
+ getdata[2]=roll;
+ return getdata;
+}
+void mpu9250_spi::AK8963_setoffset(int x,double offset)
+{
+ if((x<0)||(x>3))return;
+ Magnetometer_offset[x]+=offset;
+}
+void mpu9250_spi::AK8963_calib_Magnetometer()
+{
uint8_t response[3];
float data;
int i;
@@ -310,16 +392,17 @@
//WriteReg(MPUREG_I2C_SLV0_CTRL, 0x81); //Enable I2C and set bytes
wait(0.001);
- //response[0]=WriteReg(MPUREG_EXT_SENS_DATA_01|READ_FLAG, 0x00); //Read I2C
+ //response[0]=WriteReg(MPUREG_EXT_SENS_DATA_01|READ_FLAG, 0x00); //Read I2C
ReadRegs(MPUREG_EXT_SENS_DATA_00,response,3);
-
- //response=WriteReg(MPUREG_I2C_SLV0_DO, 0x00); //Read I2C
+
+ //response=WriteReg(MPUREG_I2C_SLV0_DO, 0x00); //Read I2C
for(i=0; i<3; i++) {
data=response[i];
Magnetometer_ASA[i]=((data-128)/256+1)*Magnetometer_Sensitivity_Scale_Factor;
}
}
-void mpu9250_spi::AK8963_read_Magnetometer(){
+void mpu9250_spi::AK8963_read_Magnetometer()
+{
uint8_t response[7];
int16_t bit_data;
float data;
@@ -335,10 +418,11 @@
for(i=0; i<3; i++) {
bit_data=((int16_t)response[i*2+1]<<8)|response[i*2];
data=(float)bit_data;
- Magnetometer[i]=data*Magnetometer_ASA[i];
+ Magnetometer[i]=data*Magnetometer_ASA[i]-Magnetometer_offset[i];
}
}
-void mpu9250_spi::read_all(){
+void mpu9250_spi::read_all()
+{
uint8_t response[21];
int16_t bit_data;
float data;
@@ -357,6 +441,8 @@
bit_data=((int16_t)response[i*2]<<8)|response[i*2+1];
data=(float)bit_data;
accelerometer_data[i]=data/acc_divider;
+ //accelerometer_data[i]=0.1*accelerometer_data[i]+0.9*accelerometer_data_prev[i];
+ //accelerometer_data_prev[i]=accelerometer_data[i];
}
//Get temperature
bit_data=((int16_t)response[i*2]<<8)|response[i*2+1];
@@ -372,19 +458,314 @@
for(i=7; i<10; i++) {
bit_data=((int16_t)response[i*2+1]<<8)|response[i*2];
data=(float)bit_data;
- Magnetometer[i-7]=data*Magnetometer_ASA[i-7];
+ Magnetometer[i-7]=data*Magnetometer_ASA[i-7]-Magnetometer_offset[i-7];
+ }
+}
+
+
+
+void mpu9250_spi::SpeedSet()
+{
+ get_speedrate();
+ read_acc();
+ /*for(int i=0;i<3;i++)
+ ratespeed[i]=acc[i];*/
+ double S[3];
+ speedT.stop();
+ for(int i=0; i<3; i++) {
+
+ S[i] =((double)(ratespeed[i]+prev_ratespeed[i])*speedT.read()/2);
+ speed[i]+=(double)S[i];
+ speed[i]=floor(speed[i]*100.0)/100.0;
+ Synthesis_speed[i]+=(double)S[i];
+ Synthesis_speed[i]=0.9*(double)(Synthesis_speed[i]+(double)ratespeed[i]/100.5)+0.1*ratespeed[i];
+ kalman_speed[i]=kalmaspeed[i].getAngle((double)kalman_speed[i], (double)ratespeed[i], (double)angleT.read()*1000);
+ comp_speed[i]=kalman_speed[i]*0.01+Synthesis_speed[i]*0.01+comp_speed[i]*0.98;
+ prev_ratespeed[i]=ratespeed[i];
+ }
+ speedT.reset();
+ speedT.start();
+}
+void mpu9250_spi::MeterSet()
+{
+ double S[3];
+ for(int i = 0 ; i<3 ; i++)
+ ratemeter[i]=comp_speed[i]*1000;
+ meterT.stop();
+ for(int i=0; i<3; i++) {
+
+ S[i] =((double)(ratemeter[i]+prev_ratemeter[i])*meterT.read()/2);
+ meter[i]+=(double)S[i];
+ Synthesis_meter[i]+=(double)S[i];
+ Synthesis_meter[i]=0.9*(double)(Synthesis_meter[i]+(double)ratemeter[i]/100.5)+0.1*meter[i];
+ kalman_meter[i]=kalmameter[i].getAngle((double)kalman_meter[i], (double)ratemeter[i], (double)angleT.read()*1000);
+ comp_meter[i]=kalman_meter[i]*0.01+Synthesis_meter[i]*0.01+comp_meter[i]*0.98;
+ prev_ratemeter[i]=ratemeter[i];
+ }
+ meterT.reset();
+ meterT.start();
+}
+
+
+
+void mpu9250_spi::MPU_setup()
+{
+ z_offset=2;
+ x_offset=0;
+ y_offset=0;
+ uint8_t buffer[6];
+
+ for(int i=0; i<sampleNum; i++) {
+ ReadRegs(MPUREG_ACCEL_XOUT_H,buffer,6);
+ int Xacc = (int)buffer[1] << 8 | (int)buffer[0];
+ int Yacc = (int)buffer[3] << 8 | (int)buffer[2];
+ int Zacc = (int)buffer[5] << 8 | (int)buffer[4]-255;
+ if((int)Xacc-x_offset>xnoise)
+ xnoise=(int)Xacc-x_offset;
+ else if((int)Xacc-x_offset<-xnoise)
+ xnoise=-(int)Xacc-x_offset;
+
+ if((int)Yacc-y_offset>ynoise)
+ ynoise=(int)Yacc-y_offset;
+ else if((int)Yacc-y_offset<-ynoise)
+ ynoise=-(int)Yacc-y_offset;
+
+ if((int)Zacc-z_offset>znoise)
+ znoise=(int)Zacc-z_offset;
+ else if((int)Zacc-z_offset<-znoise)
+ znoise=-(int)Zacc-z_offset;
+ }
+
+
+}
+void mpu9250_spi::MPU_setnum(int Num,float time,double rate)
+{
+ sampleNum=Num;
+ sampleTime=time;
+ Rate=rate;
+}
+
+
+void mpu9250_spi::get_angle_acc()
+{
+ //short data[3];
+ //get_axis_acc(data);
+ float R = sqrt(pow((float)accelerometer_data[0],2) + pow((float)accelerometer_data[1],2) + pow((float)accelerometer_data[2],2));
+ angle_acc[0]=atan2(accelerometer_data[0],accelerometer_data[2])*(180 / 3.1415);
+ angle_acc[1]=atan2(accelerometer_data[1],accelerometer_data[2])*(180 / 3.1415);
+ //angle_acc[2]=
+ /*angle_acc[1] = -((180 / 3.1415) * acos((float)accelerometer_data[1] / R)-90); // roll - angle of magnetic field vector in x direction
+ angle_acc[0] = (180 / 3.1415) * acos((float)accelerometer_data[0] / R)-90; // pitch - angle of magnetic field vector in y direction
+ angle_acc[2] = (180 / 3.1415) * acos((float)accelerometer_data[2] / R); //*/
+ /*angle_acc[0] = atan2(accelerometer_data[0],sqrt(pow((float)accelerometer_data[1],2)+pow((float)accelerometer_data[2],2)))*180/3.1415;
+ angle_acc[1] = atan2(accelerometer_data[1],sqrt(pow((float)accelerometer_data[0],2)+pow((float)accelerometer_data[2],2)))*180/3.1415;
+ angle_acc[2] = atan2(sqrt(pow((float)accelerometer_data[1],2)+pow((float)accelerometer_data[2],2)),accelerometer_data[2])*180/3.1415;*/
+
+}
+void mpu9250_spi::get_rate()
+{
+ uint8_t response[6];
+ short offset_t[3]= {0,0,0};
+ ReadRegs(MPUREG_GYRO_XOUT_H,response,6);
+ for(int i=0; i<3; i++) {
+ short bit_data=((int16_t)response[i*2]<<8)|response[i*2+1];
+ rate[i]=(double)(bit_data*0.01)-offset_t[i];
+ //printf("%d",rate[i]);
+ }
+}
+
+void mpu9250_spi::get_speedrate()
+{
+ uint8_t response[6];
+ short offset_t[3]= {0,0,0};
+ ReadRegs(MPUREG_ACCEL_XOUT_H,response,6);
+ for(int i=0; i<3; i++) {
+ short bit_data=((int16_t)response[i*2]<<8)|response[i*2+1];
+ ratespeed[i]=(short)(bit_data*0.01)-offset_t[i];
}
}
+void mpu9250_spi::get_angle(double *x,double *y,double *z)
+{
+ *x=(floor(angle[0]*100.0)/100.0);
+ *y=(floor(angle[1]*100.0)/100.0);
+ *z=(floor(angle[2]*100.0)/100.0);
+}
+void mpu9250_spi::set_angle()
+{
+ get_rate();
+ read_acc();
+ get_angle_acc();
+ double S[3];
+ angleT.stop();
+ for(int i=0; i<3; i++) {
+ //rate[i]=gyroscope_data[i]*0.01;
+ double angA=angle_acc[i];
+ S[i] =((double)(rate[i]+prev_rate[i])*angleT.read()/2);
+ // S[i]=(floor(S[i]*100.0)/100.0);//-offset_angle[i];
+ angle[i]+=(floor(S[i]*10000.0)/10000.0);//-offset_angle[i];
+ //angle[i]+=(double)S[i];
+ Synthesis_angle[i]+=(double)angle[i];
+ Synthesis_angle[i]=0.92*(double)(Synthesis_angle[i]+(double)rate[i])+0.08*angA;
+ kalman_angle[i]=kalma[i].getAngle((double)angA, (double)rate[i], (double)angleT.read_ms());
+ comp_angle[i]=kalman_angle[i];//Synthesis_angle[i]*0.5+kalman_angle[i]*0+angle[i]*0.3+angle_acc[i]*0.2;
+ comp_angle[i]=kalman_angle[i]*0.4+Synthesis_angle[i]*0.4+comp_angle[i]*0.2;
+ prev_rate[i]=rate[i];
+ }
+ angleT.reset();
+ angleT.start();
+}
+void mpu9250_spi::set_angleoffset()
+{
+ double axis[3],offseta[3];
+ offseta[0]=offseta[1]=offseta[2]=0;
+ offset_angle[0]=0;
+ offset_angle[1]=0;
+ offset_angle[2]=0;
+ for(int i=0; i<sampleNum; i++) {
+ offseta[0]+=rate[0];
+ offseta[1]+=rate[1];
+ offseta[2]+=rate[2];
+ }
+ offset_angle[0]=offseta[0]/sampleNum;
+ offset_angle[1]=offseta[1]/sampleNum;
+ offset_angle[2]=offseta[2]/sampleNum;
+ offset_angle[0]=(floor(offset_angle[0]*100.0)/100.0);
+ offset_angle[1]=(floor(offset_angle[1]*100.0)/100.0);
+ offset_angle[2]=(floor(offset_angle[2]*100.0)/100.0);
+ angle[0]=0;
+ angle[1]=0;
+ angle[2]=0;
+}
+void mpu9250_spi::set_noise()
+{
+ short gyro[3];
+ noise[0]=noise[1]=noise[2]=0;
+ uint8_t response[6];
+ for(int i=0; i<sampleNum; i++) {
+
+ for(int t=0; t<3; t++) {
+ for(int i=0; i<3; i++) {
+ short bit_data=((int16_t)response[i*2]<<8)|response[i*2+1];
+ gyro[i]=(short)bit_data;
+ }
+ if((int)gyro[t]>noise[t])
+ noise[t]=(int)gyro[t];
+ else if((int)gyro[t]<-noise[t])
+ noise[t]=-(int)gyro[t];
+ }
+ }
+ noise[0]*=sampleTime;
+ noise[1]*=sampleTime;
+ noise[2]*=sampleTime;
+}
+void mpu9250_spi::set_offset()
+{
+ short axis[3],offseta[3];
+ uint8_t response[6];
+ offseta[0]=0;
+ offseta[1]=0;
+ offseta[2]=0;
+ for(int i=0; i<sampleNum; i++) {
+ for(int i=0; i<3; i++) {
+ short bit_data=((int16_t)response[i*2]<<8)|response[i*2+1];
+ axis[i]=(short)bit_data;
+ }
+ offseta[0]+=axis[0];
+ offseta[1]+=axis[1];
+ offseta[2]+=axis[2];
+ }
+ offset[0]=offseta[0]/sampleNum;
+ offset[1]=offseta[1]/sampleNum;
+ offset[2]=offseta[2]/sampleNum;
+}
+mpu9250_spi::kalman::kalman(void)
+{
+ P[0][0] = 0;
+ P[0][1] = 0;
+ P[1][0] = 0;
+ P[1][1] = 0;
+
+ angle = 0;
+ bias = 0;
+
+ Q_angle = 0.001;
+ Q_gyroBias = 0.003;
+ R_angle = 0.03;
+}
+
+double mpu9250_spi::kalman::getAngle(double newAngle, double newRate, double dt)
+{
+ //predict the angle according to the gyroscope
+ rate = newRate - bias;
+ angle = dt * rate;
+ //update the error covariance
+ P[0][0] += dt * (dt * P[1][1] * P[0][1] - P[1][0] + Q_angle);
+ P[0][1] -= dt * P[1][1];
+ P[1][0] -= dt * P[1][1];
+ P[1][1] += dt * Q_gyroBias;
+ //difference between the predicted angle and the accelerometer angle
+ y = newAngle - angle;
+ //estimation error
+ S = P[0][0] + R_angle;
+ //determine the kalman gain according to the error covariance and the distrust
+ K[0] = P[0][0]/S;
+ K[1] = P[1][0]/S;
+ //adjust the angle according to the kalman gain and the difference with the measurement
+ angle += K[0] * y;
+ bias += K[1] * y;
+ //update the error covariance
+ P[0][0] -= K[0] * P[0][0];
+ P[0][1] -= K[0] * P[0][1];
+ P[1][0] -= K[1] * P[0][0];
+ P[1][1] -= K[1] * P[0][1];
+
+ return angle;
+}
+void mpu9250_spi::kalman::setAngle(double newAngle)
+{
+ angle = newAngle;
+};
+void mpu9250_spi::kalman::setQangle(double newQ_angle)
+{
+ Q_angle = newQ_angle;
+};
+void mpu9250_spi::kalman::setQgyroBias(double newQ_gyroBias)
+{
+ Q_gyroBias = newQ_gyroBias;
+};
+void mpu9250_spi::kalman::setRangle(double newR_angle)
+{
+ R_angle = newR_angle;
+};
+
+double mpu9250_spi::kalman::getRate(void)
+{
+ return rate;
+};
+double mpu9250_spi::kalman::getQangle(void)
+{
+ return Q_angle;
+};
+double mpu9250_spi::kalman::getQbias(void)
+{
+ return Q_gyroBias;
+};
+double mpu9250_spi::kalman::getRangle(void)
+{
+ return R_angle;
+};
/*-----------------------------------------------------------------------------------------------
SPI SELECT AND DESELECT
usage: enable and disable mpu9250 communication bus
-----------------------------------------------------------------------------------------------*/
-void mpu9250_spi::select() {
+void mpu9250_spi::select()
+{
//Set CS low to start transmission (interrupts conversion)
cs = 0;
}
-void mpu9250_spi::deselect() {
+void mpu9250_spi::deselect()
+{
//Set CS high to stop transmission (restarts conversion)
cs = 1;
}
\ No newline at end of file