Masahiro Furukawa / Mbed 2 deprecated MPU-9250-Ch2_AHRS_test

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

Fork of MPU9250_AHRS by maedalab

main.cpp@17:7a9459ac7469, 2016-06-22 (annotated)

Committer:
mfurukawa
Date:
Wed Jun 22 13:06:36 2016 +0000
Revision:
17:7a9459ac7469
Parent:
14:e795854eab64
Child:
18:a4d18f4eb968
- remove offset function; - optimized eventFunc loop; - change scale setting;

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"
mfurukawa 12:5638ddcd8477 13
mfurukawa 12:5638ddcd8477 14
mfurukawa 12:5638ddcd8477 15 /* MPU9250 Library
mfurukawa 12:5638ddcd8477 16 *
mfurukawa 12:5638ddcd8477 17 * https://developer.mbed.org/users/kylongmu/code/MPU9250_SPI_Test/file/5839d1b118bc/main.cpp
mfurukawa 17:7a9459ac7469 18 *
mfurukawa 17:7a9459ac7469 19 * MOSI (Master Out Slave In) p5
mfurukawa 17:7a9459ac7469 20 * MISO (Master In Slave Out p6
mfurukawa 17:7a9459ac7469 21 * SCK (Serial Clock) p7
mfurukawa 17:7a9459ac7469 22 * ~CS (Chip Select) p8
mfurukawa 12:5638ddcd8477 23 */
mfurukawa 10:28fa811afbfb 24
mfurukawa 17:7a9459ac7469 25 /* Madgwick AHRS Library
mfurukawa 12:5638ddcd8477 26 *
mfurukawa 12:5638ddcd8477 27 * AHRS algorithm is one of hte sensor fusion algorism.
mfurukawa 12:5638ddcd8477 28 * http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/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/
mfurukawa 12:5638ddcd8477 30 */
mfurukawa 12:5638ddcd8477 31
mfurukawa 10:28fa811afbfb 32 //define the mpu9250 object
mfurukawa 10:28fa811afbfb 33 mpu9250_spi *imu[2];
mfurukawa 17:7a9459ac7469 34
mfurukawa 17:7a9459ac7469 35 // define AHRS filters
mfurukawa 12:5638ddcd8477 36 MadgwickAHRS *ahrs[2];
mfurukawa 11:3f0b35a0855c 37
mfurukawa 17:7a9459ac7469 38 // define serial objects
mfurukawa 17:7a9459ac7469 39 Serial pc(USBTX, USBRX);
mfurukawa 11:3f0b35a0855c 40
mfurukawa 17:7a9459ac7469 41 // define SPI object for imu objects
mfurukawa 17:7a9459ac7469 42 SPI spi(p5, p6, p7);
mfurukawa 11:3f0b35a0855c 43
mfurukawa 17:7a9459ac7469 44 Ticker ticker;
mfurukawa 11:3f0b35a0855c 45
mfurukawa 11:3f0b35a0855c 46 void init(void)
mfurukawa 11:3f0b35a0855c 47 {
mfurukawa 10:28fa811afbfb 48 pc.baud(921600);
mfurukawa 11:3f0b35a0855c 49
mfurukawa 6:ea0804dc7cae 50 imu[0] = new mpu9250_spi(spi, p8);
mfurukawa 6:ea0804dc7cae 51 imu[1] = new mpu9250_spi(spi, p9);
mfurukawa 11:3f0b35a0855c 52
mfurukawa 12:5638ddcd8477 53 ahrs[0] = new MadgwickAHRS();
mfurukawa 12:5638ddcd8477 54 ahrs[1] = new MadgwickAHRS();
mfurukawa 11:3f0b35a0855c 55
mfurukawa 8:03f9b5289083 56 for(int i=0; i<2; i++) {
mfurukawa 11:3f0b35a0855c 57
mfurukawa 7:758a94e02aa7 58 imu[0]->deselect();
mfurukawa 7:758a94e02aa7 59 imu[1]->deselect();
mfurukawa 7:758a94e02aa7 60 imu[i]->select();
mfurukawa 8:03f9b5289083 61
mfurukawa 8:03f9b5289083 62 if(imu[i]->init(1,BITS_DLPF_CFG_188HZ)) { //INIT the mpu9250
mfurukawa 12:5638ddcd8477 63 printf("\nCH %d\n\nCouldn't initialize MPU9250 via SPI!", i);
mfurukawa 10:28fa811afbfb 64 wait(90);
mfurukawa 8:03f9b5289083 65 }
mfurukawa 17:7a9459ac7469 66 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
mfurukawa 17:7a9459ac7469 67 printf("Gyro_scale=%u\n",imu[i]->set_gyro_scale(BITS_FS_500DPS)); //Set 500DPS scale range for gyros
mfurukawa 17:7a9459ac7469 68 printf("Acc_scale=%u\n",imu[i]->set_acc_scale(BITS_FS_4G)); //Set 4G scale range for accs
mfurukawa 6:ea0804dc7cae 69 printf("AK8963 WHIAM=0x%2x\n",imu[i]->AK8963_whoami());
mfurukawa 6:ea0804dc7cae 70 imu[i]->AK8963_calib_Magnetometer();
mfurukawa 8:03f9b5289083 71 wait(0.1);
mfurukawa 7:758a94e02aa7 72 }
mfurukawa 10:28fa811afbfb 73 }
mfurukawa 8:03f9b5289083 74
mfurukawa 10:28fa811afbfb 75 void eventFunc(void)
mfurukawa 17:7a9459ac7469 76 {
mfurukawa 12:5638ddcd8477 77 for(int i=0; i<2; i++) {
mfurukawa 10:28fa811afbfb 78 imu[0]->deselect();
mfurukawa 10:28fa811afbfb 79 imu[1]->deselect();
mfurukawa 11:3f0b35a0855c 80
mfurukawa 10:28fa811afbfb 81 imu[i]->select();
mfurukawa 10:28fa811afbfb 82 imu[i]->read_all();
mfurukawa 12:5638ddcd8477 83 }
mfurukawa 17:7a9459ac7469 84
mfurukawa 17:7a9459ac7469 85 // update filters
mfurukawa 17:7a9459ac7469 86 for(int i=0; i<2; i++)
mfurukawa 17:7a9459ac7469 87 ahrs[i]->update(
mfurukawa 17:7a9459ac7469 88 imu[i]->gyroscope_data[0]*DEGREE2RAD,
mfurukawa 17:7a9459ac7469 89 imu[i]->gyroscope_data[1]*DEGREE2RAD,
mfurukawa 17:7a9459ac7469 90 imu[i]->gyroscope_data[2]*DEGREE2RAD,
mfurukawa 17:7a9459ac7469 91 imu[i]->accelerometer_data[0],
mfurukawa 17:7a9459ac7469 92 imu[i]->accelerometer_data[1],
mfurukawa 17:7a9459ac7469 93 imu[i]->accelerometer_data[2],
mfurukawa 17:7a9459ac7469 94 imu[i]->Magnetometer[0],
mfurukawa 17:7a9459ac7469 95 imu[i]->Magnetometer[1],
mfurukawa 17:7a9459ac7469 96 imu[i]->Magnetometer[2]
mfurukawa 10:28fa811afbfb 97 );
mfurukawa 17:7a9459ac7469 98
mfurukawa 17:7a9459ac7469 99 for(int i=0; i<2; i++)
mfurukawa 14:e795854eab64 100 printf("%+0.3f,%+0.3f,%+0.3f,%+0.3f,%+0.3f,%+0.3f,%+0.3f,",
mfurukawa 17:7a9459ac7469 101 imu[i]->gyroscope_data[0]*DEGREE2RAD,
mfurukawa 17:7a9459ac7469 102 imu[i]->gyroscope_data[1]*DEGREE2RAD,
mfurukawa 17:7a9459ac7469 103 imu[i]->gyroscope_data[2]*DEGREE2RAD,
mfurukawa 14:e795854eab64 104 ahrs[i]->q0,
mfurukawa 14:e795854eab64 105 ahrs[i]->q1,
mfurukawa 14:e795854eab64 106 ahrs[i]->q2,
mfurukawa 14:e795854eab64 107 ahrs[i]->q3);
mfurukawa 17:7a9459ac7469 108
mfurukawa 12:5638ddcd8477 109 printf("\n");
mfurukawa 10:28fa811afbfb 110 }
mfurukawa 10:28fa811afbfb 111
mfurukawa 10:28fa811afbfb 112 int main()
mfurukawa 10:28fa811afbfb 113 {
mfurukawa 17:7a9459ac7469 114 // make instances and check sensors
mfurukawa 10:28fa811afbfb 115 init();
mfurukawa 11:3f0b35a0855c 116
mfurukawa 17:7a9459ac7469 117 // define callback event
mfurukawa 11:3f0b35a0855c 118 ticker.attach_us(eventFunc, 1000000.0f/sampleFreq);
mfurukawa 11:3f0b35a0855c 119
mfurukawa 10:28fa811afbfb 120 while(1) {
mfurukawa 11:3f0b35a0855c 121
mfurukawa 11:3f0b35a0855c 122 if(pc.readable())
mfurukawa 11:3f0b35a0855c 123 if(pc.getc() == 'r') {
mfurukawa 11:3f0b35a0855c 124 ticker.detach();
mfurukawa 17:7a9459ac7469 125 // write something event here
mfurukawa 11:3f0b35a0855c 126 ticker.attach_us(eventFunc, 1000000.0f/sampleFreq);
mfurukawa 11:3f0b35a0855c 127 }
mfurukawa 11:3f0b35a0855c 128 /*
mfurukawa 11:3f0b35a0855c 129 imu[i]->read_all();
mfurukawa 11:3f0b35a0855c 130 printf("%10.3f,%10.3f,%10.3f,%10.3f,%10.3f,%10.3f,%10.3f,%10.3f,%10.3f,%10.3f ",
mfurukawa 11:3f0b35a0855c 131 imu[i]->Temperature,
mfurukawa 11:3f0b35a0855c 132 imu[i]->gyroscope_data[0],
mfurukawa 11:3f0b35a0855c 133 imu[i]->gyroscope_data[1],
mfurukawa 11:3f0b35a0855c 134 imu[i]->gyroscope_data[2],
mfurukawa 11:3f0b35a0855c 135 imu[i]->accelerometer_data[0],
mfurukawa 11:3f0b35a0855c 136 imu[i]->accelerometer_data[1],
mfurukawa 11:3f0b35a0855c 137 imu[i]->accelerometer_data[2],
mfurukawa 11:3f0b35a0855c 138 imu[i]->Magnetometer[0],
mfurukawa 11:3f0b35a0855c 139 imu[i]->Magnetometer[1],
mfurukawa 11:3f0b35a0855c 140 imu[i]->Magnetometer[2]
mfurukawa 11:3f0b35a0855c 141 );*/
mfurukawa 11:3f0b35a0855c 142 //myled = 0;
mfurukawa 11:3f0b35a0855c 143 //wait(0.5);
adisuciu 0:83fda1bfaffe 144 }
adisuciu 0:83fda1bfaffe 145 }