Masahiro Furukawa / Mbed 2 deprecated MPU-9250-Ch2

MPU-9250 with Kalman Filter

Dependencies:   ADXL362-helloworld MPU9250_SPI mbed

Fork of ADXL362-helloworld by Analog Devices

main.cpp@10:f2ef74678956, 2017-04-26 (annotated)

Committer:
mfurukawa
Date:
Wed Apr 26 07:52:10 2017 +0000
Revision:
10:f2ef74678956
Parent:
9:e700b2d586d6 
public revision

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mfurukawa 3:07aa20aa678d 1 /**
mfurukawa 3:07aa20aa678d 2 * Masahiro FURUKAWA - m.furukawa@ist.osaka-u.ac.jp
mfurukawa 8:03f9b5289083 3 *
mfurukawa 6:ea0804dc7cae 4 * June 17, 2016
mfurukawa 3:07aa20aa678d 5 *
mfurukawa 6:ea0804dc7cae 6 * MPU9250 9DoF Sensor (Extended to Ch1 ~ Ch2)
mfurukawa 3:07aa20aa678d 7 *
mfurukawa 3:07aa20aa678d 8 **/
mfurukawa 3:07aa20aa678d 9
mfurukawa 1:f1e4ee4fc335 10
adisuciu 0:83fda1bfaffe 11 #include "mbed.h"
mfurukawa 6:ea0804dc7cae 12 #include "MPU9250.h"
mfurukawa 10:f2ef74678956 13 #include "AHRS/MadgwickAHRS.h"
mfurukawa 8:03f9b5289083 14
mfurukawa 1:f1e4ee4fc335 15 /*
mfurukawa 1:f1e4ee4fc335 16 MOSI (Master Out Slave In) p5
mfurukawa 1:f1e4ee4fc335 17 MISO (Master In Slave Out p6
mfurukawa 1:f1e4ee4fc335 18 SCK (Serial Clock) p7
mfurukawa 6:ea0804dc7cae 19 ~CS (Chip Select) p8
mfurukawa 1:f1e4ee4fc335 20 */
adisuciu 0:83fda1bfaffe 21
mfurukawa 6:ea0804dc7cae 22 // https://developer.mbed.org/users/kylongmu/code/MPU9250_SPI_Test/file/5839d1b118bc/main.cpp
mfurukawa 4:5a9aa5ae928a 23
mfurukawa 8:03f9b5289083 24
mfurukawa 10:f2ef74678956 25 class KalmanFilter {
mfurukawa 10:f2ef74678956 26 private:
mfurukawa 10:f2ef74678956 27 float P, K, xhat, Q, R;
mfurukawa 10:f2ef74678956 28 public:
mfurukawa 10:f2ef74678956 29 KalmanFilter();
mfurukawa 10:f2ef74678956 30 KalmanFilter(float _Q, float _R);
mfurukawa 10:f2ef74678956 31 float update(float obs);
mfurukawa 10:f2ef74678956 32
mfurukawa 10:f2ef74678956 33 };
mfurukawa 10:f2ef74678956 34 KalmanFilter::KalmanFilter(){
mfurukawa 10:f2ef74678956 35 P = 0.0; // Convariance Matrix
mfurukawa 10:f2ef74678956 36 K = 0.0; // Kalman Gain
mfurukawa 10:f2ef74678956 37 xhat = 0.0;// Initial Predicted Value
mfurukawa 10:f2ef74678956 38 Q = 1e-3; // Error
mfurukawa 10:f2ef74678956 39 R = 0.01;
mfurukawa 10:f2ef74678956 40 }
mfurukawa 10:f2ef74678956 41
mfurukawa 10:f2ef74678956 42 // override
mfurukawa 10:f2ef74678956 43 KalmanFilter::KalmanFilter(float _Q, float _R){
mfurukawa 10:f2ef74678956 44 P = 0.0; // Convariance Matrix
mfurukawa 10:f2ef74678956 45 K = 0.0; // Kalman Gain
mfurukawa 10:f2ef74678956 46 xhat = 0.0;// Initial Predicted Value
mfurukawa 10:f2ef74678956 47 Q = _Q; // Error
mfurukawa 10:f2ef74678956 48 R = _R;
mfurukawa 10:f2ef74678956 49 }
mfurukawa 10:f2ef74678956 50
mfurukawa 10:f2ef74678956 51 float KalmanFilter::update(float obs){
mfurukawa 10:f2ef74678956 52 // Predict
mfurukawa 10:f2ef74678956 53 float xhat_m = xhat ; // xhat[k-1]
mfurukawa 10:f2ef74678956 54 float P_m = P + Q; // P[k-1]
mfurukawa 10:f2ef74678956 55
mfurukawa 10:f2ef74678956 56 // Update
mfurukawa 10:f2ef74678956 57 float S = P_m + R; // Remained Error
mfurukawa 10:f2ef74678956 58 K = P_m / S; // Update Kalman Gain
mfurukawa 10:f2ef74678956 59 xhat = xhat_m + K * (obs - xhat_m); // predicted value
mfurukawa 10:f2ef74678956 60 P = (1 - K) * P_m; // Convariance Matrix of Error BTW True Value and Predicted True Value
mfurukawa 10:f2ef74678956 61
mfurukawa 10:f2ef74678956 62 return xhat;
mfurukawa 10:f2ef74678956 63 }
mfurukawa 6:ea0804dc7cae 64
mfurukawa 10:f2ef74678956 65 //define the mpu9250 object
mfurukawa 10:f2ef74678956 66 mpu9250_spi *imu[2];
mfurukawa 10:f2ef74678956 67 Serial pc(USBTX, USBRX);
mfurukawa 10:f2ef74678956 68 SPI spi(p5, p6, p7);
mfurukawa 10:f2ef74678956 69 KalmanFilter *kf[12];
mfurukawa 10:f2ef74678956 70 Ticker ticker;
mfurukawa 10:f2ef74678956 71 float theta[2][3],thetaOffset[2][3],x,y,z;
mfurukawa 10:f2ef74678956 72
mfurukawa 10:f2ef74678956 73 // Calibration wait
mfurukawa 10:f2ef74678956 74 int count = 100;
mfurukawa 10:f2ef74678956 75
mfurukawa 10:f2ef74678956 76 // Sampling Term
mfurukawa 10:f2ef74678956 77 float smplT = 10; //ms
mfurukawa 10:f2ef74678956 78
mfurukawa 10:f2ef74678956 79 void init(void){
mfurukawa 8:03f9b5289083 80
mfurukawa 10:f2ef74678956 81 theta[0][0] = 0;
mfurukawa 10:f2ef74678956 82 theta[0][1] = 0;
mfurukawa 10:f2ef74678956 83 theta[0][2] = 0;
mfurukawa 10:f2ef74678956 84 theta[1][0] = 0;
mfurukawa 10:f2ef74678956 85 theta[1][1] = 0;
mfurukawa 10:f2ef74678956 86 theta[1][2] = 0;
mfurukawa 10:f2ef74678956 87 thetaOffset[0][0] = 0;
mfurukawa 10:f2ef74678956 88 thetaOffset[0][1] = 0;
mfurukawa 10:f2ef74678956 89 thetaOffset[0][2] = 0;
mfurukawa 10:f2ef74678956 90 thetaOffset[1][0] = 0;
mfurukawa 10:f2ef74678956 91 thetaOffset[1][1] = 0;
mfurukawa 10:f2ef74678956 92 thetaOffset[1][2] = 0;
mfurukawa 10:f2ef74678956 93
mfurukawa 10:f2ef74678956 94 pc.baud(115200);
mfurukawa 10:f2ef74678956 95
mfurukawa 6:ea0804dc7cae 96 imu[0] = new mpu9250_spi(spi, p8);
mfurukawa 6:ea0804dc7cae 97 imu[1] = new mpu9250_spi(spi, p9);
mfurukawa 10:f2ef74678956 98
mfurukawa 10:f2ef74678956 99 for(int i=0; i<12; i++)
mfurukawa 10:f2ef74678956 100 kf[i] = new KalmanFilter(1e-3, 0.001);
mfurukawa 10:f2ef74678956 101
mfurukawa 8:03f9b5289083 102 for(int i=0; i<2; i++) {
mfurukawa 10:f2ef74678956 103
mfurukawa 7:758a94e02aa7 104 imu[0]->deselect();
mfurukawa 7:758a94e02aa7 105 imu[1]->deselect();
mfurukawa 7:758a94e02aa7 106 imu[i]->select();
mfurukawa 8:03f9b5289083 107
mfurukawa 8:03f9b5289083 108 if(imu[i]->init(1,BITS_DLPF_CFG_188HZ)) { //INIT the mpu9250
mfurukawa 6:ea0804dc7cae 109 printf("\nCouldn't initialize MPU9250 via SPI!");
mfurukawa 10:f2ef74678956 110 wait(90);
mfurukawa 8:03f9b5289083 111 }
mfurukawa 6:ea0804dc7cae 112 printf("\nWHOAMI=0x%2x\n",imu[i]->whoami()); //output the I2C address to know if SPI is working, it should be 104
mfurukawa 10:f2ef74678956 113 wait(0.1);
mfurukawa 6:ea0804dc7cae 114 printf("Gyro_scale=%u\n",imu[i]->set_gyro_scale(BITS_FS_2000DPS)); //Set full scale range for gyros
mfurukawa 10:f2ef74678956 115 wait(0.1);
mfurukawa 6:ea0804dc7cae 116 printf("Acc_scale=%u\n",imu[i]->set_acc_scale(BITS_FS_16G)); //Set full scale range for accs
mfurukawa 10:f2ef74678956 117 wait(0.1);
mfurukawa 6:ea0804dc7cae 118 printf("AK8963 WHIAM=0x%2x\n",imu[i]->AK8963_whoami());
mfurukawa 8:03f9b5289083 119 wait(0.1);
mfurukawa 6:ea0804dc7cae 120 imu[i]->AK8963_calib_Magnetometer();
mfurukawa 8:03f9b5289083 121 wait(0.1);
mfurukawa 7:758a94e02aa7 122 }
mfurukawa 10:f2ef74678956 123
mfurukawa 10:f2ef74678956 124 }
mfurukawa 8:03f9b5289083 125
mfurukawa 10:f2ef74678956 126 void eventFunc(void)
mfurukawa 10:f2ef74678956 127 {
mfurukawa 10:f2ef74678956 128 count--;
mfurukawa 10:f2ef74678956 129
mfurukawa 10:f2ef74678956 130 for(int i=0; i<2; i++) {
mfurukawa 10:f2ef74678956 131
mfurukawa 10:f2ef74678956 132 imu[0]->deselect();
mfurukawa 10:f2ef74678956 133 imu[1]->deselect();
mfurukawa 8:03f9b5289083 134
mfurukawa 10:f2ef74678956 135 imu[i]->select();
mfurukawa 10:f2ef74678956 136 imu[i]->read_acc();
mfurukawa 10:f2ef74678956 137 imu[i]->read_rot();
mfurukawa 8:03f9b5289083 138
mfurukawa 10:f2ef74678956 139 x = kf[i*6 ]->update(imu[i]->gyroscope_data[0]) - thetaOffset[i][0];
mfurukawa 10:f2ef74678956 140 y = kf[i*6+1]->update(imu[i]->gyroscope_data[1]) - thetaOffset[i][1];
mfurukawa 10:f2ef74678956 141 z = kf[i*6+2]->update(imu[i]->gyroscope_data[2]) - thetaOffset[i][2];
mfurukawa 10:f2ef74678956 142 theta[i][0] += x * smplT / 1000 ; // x(n) = x(n-1) + dx*dt
mfurukawa 10:f2ef74678956 143 theta[i][1] += y * smplT / 1000 ;
mfurukawa 10:f2ef74678956 144 theta[i][2] += z * smplT / 1000 ;
mfurukawa 8:03f9b5289083 145
mfurukawa 10:f2ef74678956 146 if (count == 0){
mfurukawa 10:f2ef74678956 147 thetaOffset[i][0] = x;
mfurukawa 10:f2ef74678956 148 thetaOffset[i][1] = y;
mfurukawa 10:f2ef74678956 149 thetaOffset[i][2] = z;
mfurukawa 8:03f9b5289083 150
mfurukawa 10:f2ef74678956 151 theta[i][0] = 0;
mfurukawa 10:f2ef74678956 152 theta[i][1] = 0;
mfurukawa 10:f2ef74678956 153 theta[i][2] = 0;
mfurukawa 10:f2ef74678956 154 }
mfurukawa 10:f2ef74678956 155 if(count < 0)
mfurukawa 10:f2ef74678956 156
mfurukawa 10:f2ef74678956 157 printf("%10.3f %10.3f %10.3f %10.3f %10.3f %10.3f %10.3f %10.3f %10.3f ",
mfurukawa 10:f2ef74678956 158 x,y,z,
mfurukawa 10:f2ef74678956 159 kf[i*6+3]->update(imu[i]->accelerometer_data[0]),
mfurukawa 10:f2ef74678956 160 kf[i*6+4]->update(imu[i]->accelerometer_data[1]),
mfurukawa 10:f2ef74678956 161 kf[i*6+5]->update(imu[i]->accelerometer_data[2]),
mfurukawa 10:f2ef74678956 162 theta[i][0],
mfurukawa 10:f2ef74678956 163 theta[i][1],
mfurukawa 10:f2ef74678956 164 theta[i][2]
mfurukawa 9:e700b2d586d6 165 );
mfurukawa 10:f2ef74678956 166
mfurukawa 10:f2ef74678956 167 }
mfurukawa 10:f2ef74678956 168 printf("\n");
mfurukawa 10:f2ef74678956 169 }
mfurukawa 10:f2ef74678956 170 int main()
mfurukawa 10:f2ef74678956 171 {
mfurukawa 10:f2ef74678956 172
mfurukawa 10:f2ef74678956 173 init();
mfurukawa 10:f2ef74678956 174
mfurukawa 10:f2ef74678956 175 ticker.attach_us(eventFunc, smplT * 1000); // 10ms = 100Hz
mfurukawa 10:f2ef74678956 176
mfurukawa 10:f2ef74678956 177 while(1) {
mfurukawa 10:f2ef74678956 178
mfurukawa 9:e700b2d586d6 179 /*
mfurukawa 6:ea0804dc7cae 180 imu[i]->read_all();
mfurukawa 8:03f9b5289083 181 printf("%10.3f,%10.3f,%10.3f,%10.3f,%10.3f,%10.3f,%10.3f,%10.3f,%10.3f,%10.3f ",
mfurukawa 8:03f9b5289083 182 imu[i]->Temperature,
mfurukawa 8:03f9b5289083 183 imu[i]->gyroscope_data[0],
mfurukawa 8:03f9b5289083 184 imu[i]->gyroscope_data[1],
mfurukawa 8:03f9b5289083 185 imu[i]->gyroscope_data[2],
mfurukawa 8:03f9b5289083 186 imu[i]->accelerometer_data[0],
mfurukawa 8:03f9b5289083 187 imu[i]->accelerometer_data[1],
mfurukawa 8:03f9b5289083 188 imu[i]->accelerometer_data[2],
mfurukawa 8:03f9b5289083 189 imu[i]->Magnetometer[0],
mfurukawa 8:03f9b5289083 190 imu[i]->Magnetometer[1],
mfurukawa 8:03f9b5289083 191 imu[i]->Magnetometer[2]
mfurukawa 9:e700b2d586d6 192 );*/
mfurukawa 8:03f9b5289083 193 //myled = 0;
mfurukawa 8:03f9b5289083 194 //wait(0.5);
adisuciu 0:83fda1bfaffe 195 }
adisuciu 0:83fda1bfaffe 196 }