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@30:a1bbb934b053, 2016-07-07 (annotated)

Committer:
uribotail
Date:
Thu Jul 07 04:35:04 2016 +0000
Revision:
30:a1bbb934b053
Parent:
29:6075f35f472f
Child:
31:f30e4effec54
without magnetometer

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 27:7dd32c696d17 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 22:10de7ce8fee3 50
mfurukawa 11:3f0b35a0855c 51 void init(void)
mfurukawa 11:3f0b35a0855c 52 {
uribotail 19:3fc0ecfc723a 53 pc.baud(115200); //921600
mfurukawa 11:3f0b35a0855c 54
mfurukawa 6:ea0804dc7cae 55 imu[0] = new mpu9250_spi(spi, p8);
mfurukawa 6:ea0804dc7cae 56 imu[1] = new mpu9250_spi(spi, p9);
mfurukawa 11:3f0b35a0855c 57
uribotail 29:6075f35f472f 58 ahrs[0] = new MadgwickAHRS();
uribotail 29:6075f35f472f 59 ahrs[1] = new MadgwickAHRS();
mfurukawa 11:3f0b35a0855c 60
mfurukawa 8:03f9b5289083 61 for(int i=0; i<2; i++) {
mfurukawa 11:3f0b35a0855c 62
mfurukawa 7:758a94e02aa7 63 imu[0]->deselect();
mfurukawa 7:758a94e02aa7 64 imu[1]->deselect();
mfurukawa 7:758a94e02aa7 65 imu[i]->select();
mfurukawa 8:03f9b5289083 66
mfurukawa 8:03f9b5289083 67 if(imu[i]->init(1,BITS_DLPF_CFG_188HZ)) { //INIT the mpu9250
mfurukawa 12:5638ddcd8477 68 printf("\nCH %d\n\nCouldn't initialize MPU9250 via SPI!", i);
mfurukawa 10:28fa811afbfb 69 wait(90);
mfurukawa 8:03f9b5289083 70 }
mfurukawa 17:7a9459ac7469 71 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 72 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 73 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 74 printf("AK8963 WHIAM=0x%2x\n",imu[i]->AK8963_whoami());
mfurukawa 6:ea0804dc7cae 75 imu[i]->AK8963_calib_Magnetometer();
mfurukawa 8:03f9b5289083 76 wait(0.1);
mfurukawa 7:758a94e02aa7 77 }
mfurukawa 10:28fa811afbfb 78 }
mfurukawa 8:03f9b5289083 79
uribotail 19:3fc0ecfc723a 80 // Wada June 17, 2016
uribotail 19:3fc0ecfc723a 81 void float2byte(float gq, float k)//k = gyro_divider 32.8 or q_divider 32800
uribotail 19:3fc0ecfc723a 82 {
uribotail 20:01bba4071ffc 83 int tmp;
uribotail 19:3fc0ecfc723a 84 float omegaF = gq * k;
uribotail 19:3fc0ecfc723a 85 int16_t omegaI = (int16_t)omegaF;
uribotail 19:3fc0ecfc723a 86 if(omegaI < 0){//1の補数
uribotail 19:3fc0ecfc723a 87 omegaI = 0xFFFF + omegaI;
uribotail 19:3fc0ecfc723a 88 }
uribotail 20:01bba4071ffc 89 for(int i=0;i<4;i++){
uribotail 19:3fc0ecfc723a 90 tmp = (0xF & (omegaI >> (i*4)));
uribotail 19:3fc0ecfc723a 91 #ifdef DEBUG_putc
uribotail 19:3fc0ecfc723a 92 pc.putc(tmp);
uribotail 19:3fc0ecfc723a 93 #endif
uribotail 23:2e1ee10d4ee0 94 wait_us(waittime);
uribotail 19:3fc0ecfc723a 95 }
uribotail 19:3fc0ecfc723a 96 }//float2byte
uribotail 19:3fc0ecfc723a 97
uribotail 19:3fc0ecfc723a 98
mfurukawa 10:28fa811afbfb 99 void eventFunc(void)
mfurukawa 17:7a9459ac7469 100 {
mfurukawa 12:5638ddcd8477 101 for(int i=0; i<2; i++) {
mfurukawa 10:28fa811afbfb 102 imu[0]->deselect();
mfurukawa 10:28fa811afbfb 103 imu[1]->deselect();
mfurukawa 11:3f0b35a0855c 104
mfurukawa 10:28fa811afbfb 105 imu[i]->select();
mfurukawa 10:28fa811afbfb 106 imu[i]->read_all();
mfurukawa 12:5638ddcd8477 107 }
mfurukawa 17:7a9459ac7469 108
mfurukawa 17:7a9459ac7469 109 // update filters
mfurukawa 17:7a9459ac7469 110 for(int i=0; i<2; i++)
uribotail 29:6075f35f472f 111 {
uribotail 30:a1bbb934b053 112 imu[i]->Magnetometer[0] = 0.0f; //use updateIMU
uribotail 30:a1bbb934b053 113 imu[i]->Magnetometer[1] = 0.0f; //use updateIMU
uribotail 30:a1bbb934b053 114 imu[i]->Magnetometer[2] = 0.0f; //use updateIMU
uribotail 30:a1bbb934b053 115
uribotail 30:a1bbb934b053 116 putg[i][0] = imu[i]->gyroscope_data[0];
uribotail 30:a1bbb934b053 117 putg[i][1] = imu[i]->gyroscope_data[1];
uribotail 30:a1bbb934b053 118 putg[i][2] = imu[i]->gyroscope_data[2];
uribotail 30:a1bbb934b053 119
uribotail 29:6075f35f472f 120 ahrs[i]->update(
uribotail 30:a1bbb934b053 121 putg[i][0]*DEGREE2RAD,
uribotail 30:a1bbb934b053 122 putg[i][1]*DEGREE2RAD,
uribotail 30:a1bbb934b053 123 putg[i][2]*DEGREE2RAD,
mfurukawa 17:7a9459ac7469 124 imu[i]->accelerometer_data[0],
mfurukawa 17:7a9459ac7469 125 imu[i]->accelerometer_data[1],
mfurukawa 17:7a9459ac7469 126 imu[i]->accelerometer_data[2],
mfurukawa 17:7a9459ac7469 127 imu[i]->Magnetometer[0],
mfurukawa 17:7a9459ac7469 128 imu[i]->Magnetometer[1],
mfurukawa 17:7a9459ac7469 129 imu[i]->Magnetometer[2]
mfurukawa 10:28fa811afbfb 130 );
uribotail 29:6075f35f472f 131 }
uribotail 30:a1bbb934b053 132
uribotail 19:3fc0ecfc723a 133 #ifdef DEBUG_putc
uribotail 19:3fc0ecfc723a 134 pc.putc(0x34); //STX
uribotail 20:01bba4071ffc 135 #endif
uribotail 23:2e1ee10d4ee0 136 wait_us(waittime);
uribotail 19:3fc0ecfc723a 137 for(int i=0; i<2; i++) {
uribotail 22:10de7ce8fee3 138 putq[0] = ahrs[i]->q0;
uribotail 22:10de7ce8fee3 139 putq[1] = ahrs[i]->q1;
uribotail 22:10de7ce8fee3 140 putq[2] = ahrs[i]->q2;
uribotail 30:a1bbb934b053 141 putq[3] = ahrs[i]->q3;
uribotail 22:10de7ce8fee3 142
uribotail 30:a1bbb934b053 143 if(i==0){
uribotail 30:a1bbb934b053 144 printf("%+5.0f %+5.0f %+5.0f\t",
uribotail 30:a1bbb934b053 145 putg[i][0],
uribotail 30:a1bbb934b053 146 putg[i][1],
uribotail 30:a1bbb934b053 147 putg[i][2]);
uribotail 30:a1bbb934b053 148 printf("%+0.3f %+0.3f %+0.3f\t",
uribotail 30:a1bbb934b053 149 imu[i]->accelerometer_data[0],
uribotail 30:a1bbb934b053 150 imu[i]->accelerometer_data[1],
uribotail 30:a1bbb934b053 151 imu[i]->accelerometer_data[2]);
uribotail 30:a1bbb934b053 152 printf("%+0.3f,%+0.3f,%+0.3f,%+0.3f\t",
uribotail 22:10de7ce8fee3 153 putq[0],
uribotail 22:10de7ce8fee3 154 putq[1],
uribotail 22:10de7ce8fee3 155 putq[2],
uribotail 22:10de7ce8fee3 156 putq[3]);
uribotail 30:a1bbb934b053 157 printf("%0.3f\n",(putq[0]*putq[0])+(putq[1]*putq[1])+(putq[2]*putq[2])+(putq[3]*putq[3]));
uribotail 30:a1bbb934b053 158 }
uribotail 22:10de7ce8fee3 159
uribotail 30:a1bbb934b053 160 float2byte(putg[i][0],16.4);
uribotail 30:a1bbb934b053 161 float2byte(putg[i][1],16.4);
uribotail 30:a1bbb934b053 162 float2byte(putg[i][2],16.4);
uribotail 22:10de7ce8fee3 163 float2byte(putq[0],32800);
uribotail 22:10de7ce8fee3 164 float2byte(putq[1],32800);
uribotail 22:10de7ce8fee3 165 float2byte(putq[2],32800);
uribotail 22:10de7ce8fee3 166 float2byte(putq[3],32800);
uribotail 22:10de7ce8fee3 167
uribotail 21:06a9547359dc 168 /*
uribotail 21:06a9547359dc 169 //test signal 29,June wada
uribotail 21:06a9547359dc 170 float2byte(-200,32.8);
uribotail 21:06a9547359dc 171 float2byte( 0,32.8);
uribotail 21:06a9547359dc 172 float2byte( 200,32.8);
uribotail 21:06a9547359dc 173 float2byte(-0.2,32800);
uribotail 21:06a9547359dc 174 float2byte( 0,32800);
uribotail 21:06a9547359dc 175 float2byte( 0.4,32800);
uribotail 21:06a9547359dc 176 float2byte( 0.8,32800);
uribotail 21:06a9547359dc 177 */
uribotail 22:10de7ce8fee3 178
uribotail 19:3fc0ecfc723a 179 }
uribotail 19:3fc0ecfc723a 180 #ifdef DEBUG_putc
uribotail 19:3fc0ecfc723a 181 pc.putc(0x12); //ETX
uribotail 19:3fc0ecfc723a 182 #endif
mfurukawa 10:28fa811afbfb 183 }
mfurukawa 10:28fa811afbfb 184
mfurukawa 10:28fa811afbfb 185 int main()
mfurukawa 10:28fa811afbfb 186 {
mfurukawa 17:7a9459ac7469 187 // make instances and check sensors
mfurukawa 10:28fa811afbfb 188 init();
mfurukawa 11:3f0b35a0855c 189
mfurukawa 17:7a9459ac7469 190 // define callback event
mfurukawa 11:3f0b35a0855c 191 ticker.attach_us(eventFunc, 1000000.0f/sampleFreq);
mfurukawa 11:3f0b35a0855c 192
mfurukawa 10:28fa811afbfb 193 while(1) {
mfurukawa 11:3f0b35a0855c 194 if(pc.readable())
mfurukawa 11:3f0b35a0855c 195 if(pc.getc() == 'r') {
mfurukawa 11:3f0b35a0855c 196 ticker.detach();
mfurukawa 17:7a9459ac7469 197 // write something event here
mfurukawa 11:3f0b35a0855c 198 ticker.attach_us(eventFunc, 1000000.0f/sampleFreq);
mfurukawa 11:3f0b35a0855c 199 }
mfurukawa 11:3f0b35a0855c 200 /*
mfurukawa 11:3f0b35a0855c 201 imu[i]->read_all();
mfurukawa 11:3f0b35a0855c 202 printf("%10.3f,%10.3f,%10.3f,%10.3f,%10.3f,%10.3f,%10.3f,%10.3f,%10.3f,%10.3f ",
mfurukawa 11:3f0b35a0855c 203 imu[i]->Temperature,
mfurukawa 11:3f0b35a0855c 204 imu[i]->gyroscope_data[0],
mfurukawa 11:3f0b35a0855c 205 imu[i]->gyroscope_data[1],
mfurukawa 11:3f0b35a0855c 206 imu[i]->gyroscope_data[2],
mfurukawa 11:3f0b35a0855c 207 imu[i]->accelerometer_data[0],
mfurukawa 11:3f0b35a0855c 208 imu[i]->accelerometer_data[1],
mfurukawa 11:3f0b35a0855c 209 imu[i]->accelerometer_data[2],
mfurukawa 11:3f0b35a0855c 210 imu[i]->Magnetometer[0],
mfurukawa 11:3f0b35a0855c 211 imu[i]->Magnetometer[1],
mfurukawa 11:3f0b35a0855c 212 imu[i]->Magnetometer[2]
mfurukawa 11:3f0b35a0855c 213 );*/
mfurukawa 11:3f0b35a0855c 214 //myled = 0;
mfurukawa 11:3f0b35a0855c 215 //wait(0.5);
adisuciu 0:83fda1bfaffe 216 }
adisuciu 0:83fda1bfaffe 217 }