maedalab / Mbed 2 deprecated MPU9250_AHRS

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:   MPU9250_SPI mbed

main.cpp@31:f30e4effec54, 2016-07-07 (annotated)

Committer:
uribotail
Date:
Thu Jul 07 06:04:05 2016 +0000
Revision:
31:f30e4effec54
Parent:
30:a1bbb934b053 
drift corrected

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
adisuciu 0:83fda1bfaffe 10 #include "mbed.h"
mfurukawa 6:ea0804dc7cae 11 #include "MPU9250.h"
mfurukawa 17:7a9459ac7469 12 #include "MadgwickAHRS.h"
uribotail 31:f30e4effec54 13 //#include "MahonyAHRS.h"
uribotail 29:6075f35f472f 14 //#define DEBUG_putc //Wada
mfurukawa 12:5638ddcd8477 15
mfurukawa 12:5638ddcd8477 16 /* MPU9250 Library
mfurukawa 12:5638ddcd8477 17 *
mfurukawa 12:5638ddcd8477 18 * https://developer.mbed.org/users/kylongmu/code/MPU9250_SPI_Test/file/5839d1b118bc/main.cpp
mfurukawa 17:7a9459ac7469 19 *
mfurukawa 17:7a9459ac7469 20 * MOSI (Master Out Slave In) p5
mfurukawa 17:7a9459ac7469 21 * MISO (Master In Slave Out p6
mfurukawa 17:7a9459ac7469 22 * SCK (Serial Clock) p7
mfurukawa 17:7a9459ac7469 23 * ~CS (Chip Select) p8
mfurukawa 12:5638ddcd8477 24 */
mfurukawa 10:28fa811afbfb 25
mfurukawa 17:7a9459ac7469 26 /* Madgwick AHRS Library
mfurukawa 12:5638ddcd8477 27 *
mfurukawa 12:5638ddcd8477 28 * AHRS algorithm is one of hte sensor fusion algorism.
mfurukawa 12:5638ddcd8477 29 * http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/AHRS algorithm is one of hte sensor fusion algorism.
mfurukawa 12:5638ddcd8477 30 * http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/
mfurukawa 12:5638ddcd8477 31 */
mfurukawa 12:5638ddcd8477 32
mfurukawa 10:28fa811afbfb 33 //define the mpu9250 object
mfurukawa 10:28fa811afbfb 34 mpu9250_spi *imu[2];
mfurukawa 17:7a9459ac7469 35
mfurukawa 17:7a9459ac7469 36 // define AHRS filters
uribotail 29:6075f35f472f 37 MadgwickAHRS *ahrs[2];
mfurukawa 11:3f0b35a0855c 38
mfurukawa 17:7a9459ac7469 39 // define serial objects
mfurukawa 17:7a9459ac7469 40 Serial pc(USBTX, USBRX);
mfurukawa 11:3f0b35a0855c 41
mfurukawa 17:7a9459ac7469 42 // define SPI object for imu objects
mfurukawa 17:7a9459ac7469 43 SPI spi(p5, p6, p7);
mfurukawa 11:3f0b35a0855c 44
mfurukawa 17:7a9459ac7469 45 Ticker ticker;
mfurukawa 11:3f0b35a0855c 46
uribotail 30:a1bbb934b053 47 float putg[2][3] = {0};
uribotail 22:10de7ce8fee3 48 float putq[4] = {0};
uribotail 23:2e1ee10d4ee0 49 int waittime = 80;
uribotail 31:f30e4effec54 50 int count = 0;
uribotail 31:f30e4effec54 51 float calibg[2][3] = {0};
uribotail 31:f30e4effec54 52 float caliba[2][3] = {0};
uribotail 31:f30e4effec54 53
uribotail 22:10de7ce8fee3 54
mfurukawa 11:3f0b35a0855c 55 void init(void)
mfurukawa 11:3f0b35a0855c 56 {
uribotail 19:3fc0ecfc723a 57 pc.baud(115200); //921600
mfurukawa 11:3f0b35a0855c 58
mfurukawa 6:ea0804dc7cae 59 imu[0] = new mpu9250_spi(spi, p8);
mfurukawa 6:ea0804dc7cae 60 imu[1] = new mpu9250_spi(spi, p9);
mfurukawa 11:3f0b35a0855c 61
uribotail 29:6075f35f472f 62 ahrs[0] = new MadgwickAHRS();
uribotail 29:6075f35f472f 63 ahrs[1] = new MadgwickAHRS();
mfurukawa 11:3f0b35a0855c 64
mfurukawa 8:03f9b5289083 65 for(int i=0; i<2; i++) {
mfurukawa 11:3f0b35a0855c 66
mfurukawa 7:758a94e02aa7 67 imu[0]->deselect();
mfurukawa 7:758a94e02aa7 68 imu[1]->deselect();
mfurukawa 7:758a94e02aa7 69 imu[i]->select();
mfurukawa 8:03f9b5289083 70
mfurukawa 8:03f9b5289083 71 if(imu[i]->init(1,BITS_DLPF_CFG_188HZ)) { //INIT the mpu9250
mfurukawa 12:5638ddcd8477 72 printf("\nCH %d\n\nCouldn't initialize MPU9250 via SPI!", i);
mfurukawa 10:28fa811afbfb 73 wait(90);
mfurukawa 8:03f9b5289083 74 }
mfurukawa 17:7a9459ac7469 75 printf("\nCH %d\nWHOAMI=0x%2x\n",i, imu[i]->whoami()); //output the I2C address to know if SPI is working, it should be 104
uribotail 25:f8fd336981cb 76 printf("Gyro_scale=%u\n",imu[i]->set_gyro_scale(BITS_FS_2000DPS)); //Set 500DPS scale range for gyros //0706 wada 500to2000
uribotail 25:f8fd336981cb 77 printf("Acc_scale=%u\n",imu[i]->set_acc_scale(BITS_FS_16G)); //Set 4G scale range for accs //0706 wada 4to16
mfurukawa 6:ea0804dc7cae 78 printf("AK8963 WHIAM=0x%2x\n",imu[i]->AK8963_whoami());
mfurukawa 6:ea0804dc7cae 79 imu[i]->AK8963_calib_Magnetometer();
mfurukawa 8:03f9b5289083 80 wait(0.1);
mfurukawa 7:758a94e02aa7 81 }
mfurukawa 10:28fa811afbfb 82 }
mfurukawa 8:03f9b5289083 83
uribotail 19:3fc0ecfc723a 84 // Wada June 17, 2016
uribotail 19:3fc0ecfc723a 85 void float2byte(float gq, float k)//k = gyro_divider 32.8 or q_divider 32800
uribotail 19:3fc0ecfc723a 86 {
uribotail 20:01bba4071ffc 87 int tmp;
uribotail 19:3fc0ecfc723a 88 float omegaF = gq * k;
uribotail 19:3fc0ecfc723a 89 int16_t omegaI = (int16_t)omegaF;
uribotail 19:3fc0ecfc723a 90 if(omegaI < 0){//1の補数
uribotail 19:3fc0ecfc723a 91 omegaI = 0xFFFF + omegaI;
uribotail 19:3fc0ecfc723a 92 }
uribotail 20:01bba4071ffc 93 for(int i=0;i<4;i++){
uribotail 19:3fc0ecfc723a 94 tmp = (0xF & (omegaI >> (i*4)));
uribotail 19:3fc0ecfc723a 95 #ifdef DEBUG_putc
uribotail 19:3fc0ecfc723a 96 pc.putc(tmp);
uribotail 19:3fc0ecfc723a 97 #endif
uribotail 23:2e1ee10d4ee0 98 wait_us(waittime);
uribotail 19:3fc0ecfc723a 99 }
uribotail 19:3fc0ecfc723a 100 }//float2byte
uribotail 19:3fc0ecfc723a 101
uribotail 19:3fc0ecfc723a 102
uribotail 31:f30e4effec54 103 void calibFunc(void)
uribotail 31:f30e4effec54 104 {
uribotail 31:f30e4effec54 105 for(int i=0; i<2; i++) {
uribotail 31:f30e4effec54 106 imu[0]->deselect();
uribotail 31:f30e4effec54 107 imu[1]->deselect();
uribotail 31:f30e4effec54 108 imu[i]->select();
uribotail 31:f30e4effec54 109 imu[i]->read_all();
uribotail 31:f30e4effec54 110 calibg[i][0] = (calibg[i][0] + imu[i]->gyroscope_data[0])/2;
uribotail 31:f30e4effec54 111 calibg[i][1] = (calibg[i][1] + imu[i]->gyroscope_data[1])/2;
uribotail 31:f30e4effec54 112 calibg[i][2] = (calibg[i][2] + imu[i]->gyroscope_data[2])/2;
uribotail 31:f30e4effec54 113 //caliba[i][0] = (caliba[i][0] + imu[i]->accelerometer_data[0])/2;
uribotail 31:f30e4effec54 114 //caliba[i][1] = (caliba[i][1] + imu[i]->accelerometer_data[1])/2;
uribotail 31:f30e4effec54 115 //caliba[i][2] = (caliba[i][2] + imu[i]->accelerometer_data[2])/2;
uribotail 31:f30e4effec54 116 }
uribotail 31:f30e4effec54 117 printf("calib = %f \n",calibg[1][0]);
uribotail 31:f30e4effec54 118 }
uribotail 31:f30e4effec54 119
uribotail 31:f30e4effec54 120
mfurukawa 10:28fa811afbfb 121 void eventFunc(void)
mfurukawa 17:7a9459ac7469 122 {
mfurukawa 12:5638ddcd8477 123 for(int i=0; i<2; i++) {
mfurukawa 10:28fa811afbfb 124 imu[0]->deselect();
mfurukawa 10:28fa811afbfb 125 imu[1]->deselect();
mfurukawa 11:3f0b35a0855c 126
mfurukawa 10:28fa811afbfb 127 imu[i]->select();
mfurukawa 10:28fa811afbfb 128 imu[i]->read_all();
mfurukawa 12:5638ddcd8477 129 }
mfurukawa 17:7a9459ac7469 130
mfurukawa 17:7a9459ac7469 131 // update filters
mfurukawa 17:7a9459ac7469 132 for(int i=0; i<2; i++)
uribotail 29:6075f35f472f 133 {
uribotail 30:a1bbb934b053 134 imu[i]->Magnetometer[0] = 0.0f; //use updateIMU
uribotail 30:a1bbb934b053 135 imu[i]->Magnetometer[1] = 0.0f; //use updateIMU
uribotail 30:a1bbb934b053 136 imu[i]->Magnetometer[2] = 0.0f; //use updateIMU
uribotail 30:a1bbb934b053 137
uribotail 31:f30e4effec54 138 putg[i][0] = imu[i]->gyroscope_data[0] - calibg[i][0];
uribotail 31:f30e4effec54 139 putg[i][1] = imu[i]->gyroscope_data[1] - calibg[i][1];
uribotail 31:f30e4effec54 140 putg[i][2] = imu[i]->gyroscope_data[2] - calibg[i][2];
uribotail 30:a1bbb934b053 141
uribotail 29:6075f35f472f 142 ahrs[i]->update(
uribotail 30:a1bbb934b053 143 putg[i][0]*DEGREE2RAD,
uribotail 30:a1bbb934b053 144 putg[i][1]*DEGREE2RAD,
uribotail 30:a1bbb934b053 145 putg[i][2]*DEGREE2RAD,
uribotail 31:f30e4effec54 146 imu[i]->accelerometer_data[0] - caliba[i][0],
uribotail 31:f30e4effec54 147 imu[i]->accelerometer_data[1] - caliba[i][1],
uribotail 31:f30e4effec54 148 imu[i]->accelerometer_data[2] - caliba[i][2],
mfurukawa 17:7a9459ac7469 149 imu[i]->Magnetometer[0],
mfurukawa 17:7a9459ac7469 150 imu[i]->Magnetometer[1],
mfurukawa 17:7a9459ac7469 151 imu[i]->Magnetometer[2]
mfurukawa 10:28fa811afbfb 152 );
uribotail 29:6075f35f472f 153 }
uribotail 30:a1bbb934b053 154
uribotail 19:3fc0ecfc723a 155 #ifdef DEBUG_putc
uribotail 19:3fc0ecfc723a 156 pc.putc(0x34); //STX
uribotail 20:01bba4071ffc 157 #endif
uribotail 23:2e1ee10d4ee0 158 wait_us(waittime);
uribotail 31:f30e4effec54 159 if(count==4){ //sample500Hzに対してprintf100Hz
uribotail 31:f30e4effec54 160 for(int i=0; i<2; i++) {
uribotail 31:f30e4effec54 161 putq[0] = ahrs[i]->q0;
uribotail 31:f30e4effec54 162 putq[1] = ahrs[i]->q1;
uribotail 31:f30e4effec54 163 putq[2] = ahrs[i]->q2;
uribotail 31:f30e4effec54 164 putq[3] = ahrs[i]->q3;
uribotail 31:f30e4effec54 165
uribotail 31:f30e4effec54 166 if(i==1){
uribotail 31:f30e4effec54 167 printf("%+5.0f %+5.0f %+5.0f\t",
uribotail 31:f30e4effec54 168 putg[i][0],
uribotail 31:f30e4effec54 169 putg[i][1],
uribotail 31:f30e4effec54 170 putg[i][2]);
uribotail 31:f30e4effec54 171 printf("%+0.3f %+0.3f %+0.3f\t",
uribotail 31:f30e4effec54 172 imu[i]->accelerometer_data[0] - caliba[i][0],
uribotail 31:f30e4effec54 173 imu[i]->accelerometer_data[1] - caliba[i][1],
uribotail 31:f30e4effec54 174 imu[i]->accelerometer_data[2] - caliba[i][2]);
uribotail 31:f30e4effec54 175 printf("%+0.3f,%+0.3f,%+0.3f,%+0.3f\t",
uribotail 31:f30e4effec54 176 putq[0],
uribotail 31:f30e4effec54 177 putq[1],
uribotail 31:f30e4effec54 178 putq[2],
uribotail 31:f30e4effec54 179 putq[3]);
uribotail 31:f30e4effec54 180 printf("%0.3f\n",(putq[0]*putq[0])+(putq[1]*putq[1])+(putq[2]*putq[2])+(putq[3]*putq[3]));
uribotail 31:f30e4effec54 181 }
uribotail 31:f30e4effec54 182
uribotail 31:f30e4effec54 183 float2byte(putg[i][0],16.4);
uribotail 31:f30e4effec54 184 float2byte(putg[i][1],16.4);
uribotail 31:f30e4effec54 185 float2byte(putg[i][2],16.4);
uribotail 31:f30e4effec54 186 float2byte(putq[0],32800);
uribotail 31:f30e4effec54 187 float2byte(putq[1],32800);
uribotail 31:f30e4effec54 188 float2byte(putq[2],32800);
uribotail 31:f30e4effec54 189 float2byte(putq[3],32800);
uribotail 31:f30e4effec54 190
uribotail 31:f30e4effec54 191 /*
uribotail 31:f30e4effec54 192 //test signal 29,June wada
uribotail 31:f30e4effec54 193 float2byte(-200,32.8);
uribotail 31:f30e4effec54 194 float2byte( 0,32.8);
uribotail 31:f30e4effec54 195 float2byte( 200,32.8);
uribotail 31:f30e4effec54 196 float2byte(-0.2,32800);
uribotail 31:f30e4effec54 197 float2byte( 0,32800);
uribotail 31:f30e4effec54 198 float2byte( 0.4,32800);
uribotail 31:f30e4effec54 199 float2byte( 0.8,32800);
uribotail 31:f30e4effec54 200 */
uribotail 31:f30e4effec54 201 }
uribotail 19:3fc0ecfc723a 202 #ifdef DEBUG_putc
uribotail 19:3fc0ecfc723a 203 pc.putc(0x12); //ETX
uribotail 31:f30e4effec54 204 #endif
uribotail 31:f30e4effec54 205
uribotail 31:f30e4effec54 206 count = 0;
uribotail 31:f30e4effec54 207 }//count
uribotail 31:f30e4effec54 208 count++;
mfurukawa 10:28fa811afbfb 209 }
mfurukawa 10:28fa811afbfb 210
mfurukawa 10:28fa811afbfb 211 int main()
mfurukawa 10:28fa811afbfb 212 {
mfurukawa 17:7a9459ac7469 213 // make instances and check sensors
mfurukawa 10:28fa811afbfb 214 init();
uribotail 31:f30e4effec54 215
uribotail 31:f30e4effec54 216 ticker.attach_us(calibFunc, 1000000.0f/sampleFreq);
mfurukawa 11:3f0b35a0855c 217
uribotail 31:f30e4effec54 218 for(int i=0; i<1000000; i++){
uribotail 31:f30e4effec54 219 if(pc.readable())
uribotail 31:f30e4effec54 220 if(pc.getc() == 'r') {
uribotail 31:f30e4effec54 221 ticker.detach();
uribotail 31:f30e4effec54 222 // write something event here
uribotail 31:f30e4effec54 223 ticker.attach_us(calibFunc, 1000000.0f/sampleFreq);
uribotail 31:f30e4effec54 224 }
uribotail 31:f30e4effec54 225 }
uribotail 31:f30e4effec54 226
uribotail 31:f30e4effec54 227
uribotail 31:f30e4effec54 228 ticker.detach();
mfurukawa 17:7a9459ac7469 229 // define callback event
mfurukawa 11:3f0b35a0855c 230 ticker.attach_us(eventFunc, 1000000.0f/sampleFreq);
uribotail 31:f30e4effec54 231
mfurukawa 10:28fa811afbfb 232 while(1) {
mfurukawa 11:3f0b35a0855c 233 if(pc.readable())
mfurukawa 11:3f0b35a0855c 234 if(pc.getc() == 'r') {
mfurukawa 11:3f0b35a0855c 235 ticker.detach();
mfurukawa 17:7a9459ac7469 236 // write something event here
mfurukawa 11:3f0b35a0855c 237 ticker.attach_us(eventFunc, 1000000.0f/sampleFreq);
mfurukawa 11:3f0b35a0855c 238 }
mfurukawa 11:3f0b35a0855c 239 /*
mfurukawa 11:3f0b35a0855c 240 imu[i]->read_all();
mfurukawa 11:3f0b35a0855c 241 printf("%10.3f,%10.3f,%10.3f,%10.3f,%10.3f,%10.3f,%10.3f,%10.3f,%10.3f,%10.3f ",
mfurukawa 11:3f0b35a0855c 242 imu[i]->Temperature,
mfurukawa 11:3f0b35a0855c 243 imu[i]->gyroscope_data[0],
mfurukawa 11:3f0b35a0855c 244 imu[i]->gyroscope_data[1],
mfurukawa 11:3f0b35a0855c 245 imu[i]->gyroscope_data[2],
mfurukawa 11:3f0b35a0855c 246 imu[i]->accelerometer_data[0],
mfurukawa 11:3f0b35a0855c 247 imu[i]->accelerometer_data[1],
mfurukawa 11:3f0b35a0855c 248 imu[i]->accelerometer_data[2],
mfurukawa 11:3f0b35a0855c 249 imu[i]->Magnetometer[0],
mfurukawa 11:3f0b35a0855c 250 imu[i]->Magnetometer[1],
mfurukawa 11:3f0b35a0855c 251 imu[i]->Magnetometer[2]
mfurukawa 11:3f0b35a0855c 252 );*/
mfurukawa 11:3f0b35a0855c 253 //myled = 0;
mfurukawa 11:3f0b35a0855c 254 //wait(0.5);
adisuciu 0:83fda1bfaffe 255 }
adisuciu 0:83fda1bfaffe 256 }