fork
Dependencies: MPU9250_SPI mbed
Fork of MPU9250_AHRS by
main.cpp@29:6075f35f472f, 2016-07-06 (annotated)
- Committer:
- uribotail
- Date:
- Wed Jul 06 11:44:22 2016 +0000
- Revision:
- 29:6075f35f472f
- Parent:
- 28:76e2ba7a1ecd
20:44wada debag now
Who changed what in which revision?
User | Revision | Line number | New 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 | 22:10de7ce8fee3 | 47 | float putg[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 | 29:6075f35f472f | 112 | ahrs[i]->update( |
mfurukawa | 17:7a9459ac7469 | 113 | imu[i]->gyroscope_data[0]*DEGREE2RAD, |
mfurukawa | 17:7a9459ac7469 | 114 | imu[i]->gyroscope_data[1]*DEGREE2RAD, |
mfurukawa | 17:7a9459ac7469 | 115 | imu[i]->gyroscope_data[2]*DEGREE2RAD, |
mfurukawa | 17:7a9459ac7469 | 116 | imu[i]->accelerometer_data[0], |
mfurukawa | 17:7a9459ac7469 | 117 | imu[i]->accelerometer_data[1], |
mfurukawa | 17:7a9459ac7469 | 118 | imu[i]->accelerometer_data[2], |
mfurukawa | 17:7a9459ac7469 | 119 | imu[i]->Magnetometer[0], |
mfurukawa | 17:7a9459ac7469 | 120 | imu[i]->Magnetometer[1], |
mfurukawa | 17:7a9459ac7469 | 121 | imu[i]->Magnetometer[2] |
mfurukawa | 10:28fa811afbfb | 122 | ); |
uribotail | 29:6075f35f472f | 123 | } |
uribotail | 29:6075f35f472f | 124 | /* printf("%+4.3f %+4.3f %+4.3f ", |
uribotail | 29:6075f35f472f | 125 | imu[1]->Magnetometer[0], |
uribotail | 29:6075f35f472f | 126 | imu[1]->Magnetometer[1], |
uribotail | 29:6075f35f472f | 127 | imu[1]->Magnetometer[2] |
uribotail | 29:6075f35f472f | 128 | ); |
uribotail | 29:6075f35f472f | 129 | printf("\n"); |
uribotail | 29:6075f35f472f | 130 | */ |
uribotail | 19:3fc0ecfc723a | 131 | #ifdef DEBUG_putc |
uribotail | 19:3fc0ecfc723a | 132 | pc.putc(0x34); //STX |
uribotail | 20:01bba4071ffc | 133 | #endif |
uribotail | 23:2e1ee10d4ee0 | 134 | wait_us(waittime); |
uribotail | 19:3fc0ecfc723a | 135 | for(int i=0; i<2; i++) { |
uribotail | 22:10de7ce8fee3 | 136 | putg[0] = imu[i]->gyroscope_data[0]; |
uribotail | 22:10de7ce8fee3 | 137 | putg[1] = imu[i]->gyroscope_data[1]; |
uribotail | 22:10de7ce8fee3 | 138 | putg[2] = imu[i]->gyroscope_data[2]; |
uribotail | 22:10de7ce8fee3 | 139 | putq[0] = ahrs[i]->q0; |
uribotail | 22:10de7ce8fee3 | 140 | putq[1] = ahrs[i]->q1; |
uribotail | 22:10de7ce8fee3 | 141 | putq[2] = ahrs[i]->q2; |
uribotail | 22:10de7ce8fee3 | 142 | putq[3] = ahrs[i]->q3; |
uribotail | 22:10de7ce8fee3 | 143 | |
uribotail | 19:3fc0ecfc723a | 144 | /* |
uribotail | 18:a4d18f4eb968 | 145 | printf("%+0.3f,%+0.3f,%+0.3f,%+0.3f,%+0.3f,%+0.3f,%+0.3f,", |
uribotail | 22:10de7ce8fee3 | 146 | putg[0]*DEGREE2RAD, |
uribotail | 22:10de7ce8fee3 | 147 | putg[1]*DEGREE2RAD, |
uribotail | 22:10de7ce8fee3 | 148 | putg[2]*DEGREE2RAD, |
uribotail | 22:10de7ce8fee3 | 149 | putq[0], |
uribotail | 22:10de7ce8fee3 | 150 | putq[1], |
uribotail | 22:10de7ce8fee3 | 151 | putq[2], |
uribotail | 22:10de7ce8fee3 | 152 | putq[3]); |
uribotail | 19:3fc0ecfc723a | 153 | */ |
uribotail | 22:10de7ce8fee3 | 154 | |
uribotail | 26:0ffbe34a4953 | 155 | float2byte(putg[0],16.4); |
uribotail | 26:0ffbe34a4953 | 156 | float2byte(putg[1],16.4); |
uribotail | 26:0ffbe34a4953 | 157 | float2byte(putg[2],16.4); |
uribotail | 22:10de7ce8fee3 | 158 | float2byte(putq[0],32800); |
uribotail | 22:10de7ce8fee3 | 159 | float2byte(putq[1],32800); |
uribotail | 22:10de7ce8fee3 | 160 | float2byte(putq[2],32800); |
uribotail | 22:10de7ce8fee3 | 161 | float2byte(putq[3],32800); |
uribotail | 22:10de7ce8fee3 | 162 | |
uribotail | 21:06a9547359dc | 163 | /* |
uribotail | 21:06a9547359dc | 164 | //test signal 29,June wada |
uribotail | 21:06a9547359dc | 165 | float2byte(-200,32.8); |
uribotail | 21:06a9547359dc | 166 | float2byte( 0,32.8); |
uribotail | 21:06a9547359dc | 167 | float2byte( 200,32.8); |
uribotail | 21:06a9547359dc | 168 | float2byte(-0.2,32800); |
uribotail | 21:06a9547359dc | 169 | float2byte( 0,32800); |
uribotail | 21:06a9547359dc | 170 | float2byte( 0.4,32800); |
uribotail | 21:06a9547359dc | 171 | float2byte( 0.8,32800); |
uribotail | 21:06a9547359dc | 172 | */ |
uribotail | 22:10de7ce8fee3 | 173 | |
uribotail | 19:3fc0ecfc723a | 174 | } |
uribotail | 19:3fc0ecfc723a | 175 | #ifdef DEBUG_putc |
uribotail | 19:3fc0ecfc723a | 176 | pc.putc(0x12); //ETX |
uribotail | 19:3fc0ecfc723a | 177 | #endif |
uribotail | 23:2e1ee10d4ee0 | 178 | //printf("\n"); |
mfurukawa | 10:28fa811afbfb | 179 | } |
mfurukawa | 10:28fa811afbfb | 180 | |
mfurukawa | 10:28fa811afbfb | 181 | int main() |
mfurukawa | 10:28fa811afbfb | 182 | { |
mfurukawa | 17:7a9459ac7469 | 183 | // make instances and check sensors |
mfurukawa | 10:28fa811afbfb | 184 | init(); |
mfurukawa | 11:3f0b35a0855c | 185 | |
mfurukawa | 17:7a9459ac7469 | 186 | // define callback event |
mfurukawa | 11:3f0b35a0855c | 187 | ticker.attach_us(eventFunc, 1000000.0f/sampleFreq); |
mfurukawa | 11:3f0b35a0855c | 188 | |
mfurukawa | 10:28fa811afbfb | 189 | while(1) { |
mfurukawa | 11:3f0b35a0855c | 190 | |
mfurukawa | 11:3f0b35a0855c | 191 | if(pc.readable()) |
mfurukawa | 11:3f0b35a0855c | 192 | if(pc.getc() == 'r') { |
mfurukawa | 11:3f0b35a0855c | 193 | ticker.detach(); |
mfurukawa | 17:7a9459ac7469 | 194 | // write something event here |
mfurukawa | 11:3f0b35a0855c | 195 | ticker.attach_us(eventFunc, 1000000.0f/sampleFreq); |
mfurukawa | 11:3f0b35a0855c | 196 | } |
mfurukawa | 11:3f0b35a0855c | 197 | /* |
mfurukawa | 11:3f0b35a0855c | 198 | imu[i]->read_all(); |
mfurukawa | 11:3f0b35a0855c | 199 | printf("%10.3f,%10.3f,%10.3f,%10.3f,%10.3f,%10.3f,%10.3f,%10.3f,%10.3f,%10.3f ", |
mfurukawa | 11:3f0b35a0855c | 200 | imu[i]->Temperature, |
mfurukawa | 11:3f0b35a0855c | 201 | imu[i]->gyroscope_data[0], |
mfurukawa | 11:3f0b35a0855c | 202 | imu[i]->gyroscope_data[1], |
mfurukawa | 11:3f0b35a0855c | 203 | imu[i]->gyroscope_data[2], |
mfurukawa | 11:3f0b35a0855c | 204 | imu[i]->accelerometer_data[0], |
mfurukawa | 11:3f0b35a0855c | 205 | imu[i]->accelerometer_data[1], |
mfurukawa | 11:3f0b35a0855c | 206 | imu[i]->accelerometer_data[2], |
mfurukawa | 11:3f0b35a0855c | 207 | imu[i]->Magnetometer[0], |
mfurukawa | 11:3f0b35a0855c | 208 | imu[i]->Magnetometer[1], |
mfurukawa | 11:3f0b35a0855c | 209 | imu[i]->Magnetometer[2] |
mfurukawa | 11:3f0b35a0855c | 210 | );*/ |
mfurukawa | 11:3f0b35a0855c | 211 | //myled = 0; |
mfurukawa | 11:3f0b35a0855c | 212 | //wait(0.5); |
adisuciu | 0:83fda1bfaffe | 213 | } |
adisuciu | 0:83fda1bfaffe | 214 | } |