Masahiro Furukawa / Mbed 2 deprecated MPU-9250-MagSens

2Chx3dof Magnetrometer supported

Dependencies:   mbed

main.cpp@0:4656a133ed1a, 2021-02-01 (annotated)

Committer:
mfurukawa
Date:
Mon Feb 01 17:11:03 2021 +0000
Revision:
0:4656a133ed1a
initial commit;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mfurukawa 0:4656a133ed1a 1 /**
mfurukawa 0:4656a133ed1a 2 * Masahiro FURUKAWA - m.furukawa@ist.osaka-u.ac.jp
mfurukawa 0:4656a133ed1a 3 *
mfurukawa 0:4656a133ed1a 4 * Dec 26, 2017
mfurukawa 0:4656a133ed1a 5 * Aug 29, 2018
mfurukawa 0:4656a133ed1a 6 * Dec 17, 2019 @ Digital Low Pass Filter 5Hz -> No LPF
mfurukawa 0:4656a133ed1a 7 @ ACC range 2G -> 4G
mfurukawa 0:4656a133ed1a 8 @ GYRO range 250 -> 500 Degree per second
mfurukawa 0:4656a133ed1a 9 @ Deleted magnet sensor checking
mfurukawa 0:4656a133ed1a 10 @ Set Acc Data Rates, Enable Acc LPF , Bandwidth 218Hz
mfurukawa 0:4656a133ed1a 11 @ Use DLPF set Gyroscope bandwidth 5Hz, temperature bandwidth 5Hz
mfurukawa 0:4656a133ed1a 12 * Feb 1, 2021 @Magnetro Meter Ch1
mfurukawa 0:4656a133ed1a 13 *
mfurukawa 0:4656a133ed1a 14 * MPU9250 9DoF Sensor (Extended to Ch1 ~ Ch4)
mfurukawa 0:4656a133ed1a 15 *
mfurukawa 0:4656a133ed1a 16 **/
mfurukawa 0:4656a133ed1a 17
mfurukawa 0:4656a133ed1a 18 /*
mfurukawa 0:4656a133ed1a 19 https://invensense.tdk.com/wp-content/uploads/2015/02/PS-MPU-9250A-01-v1.1.pdf
mfurukawa 0:4656a133ed1a 20
mfurukawa 0:4656a133ed1a 21 3.3 Magnetometer Specifications
mfurukawa 0:4656a133ed1a 22
mfurukawa 0:4656a133ed1a 23 Typical Operating Circuit of section 4.2,
mfurukawa 0:4656a133ed1a 24 VDD = 2.5V,
mfurukawa 0:4656a133ed1a 25 VDDIO = 2.5V,
mfurukawa 0:4656a133ed1a 26 TA=25°C, unless otherwise noted.
mfurukawa 0:4656a133ed1a 27
mfurukawa 0:4656a133ed1a 28 PARAMETER CONDITIONS MIN TYP MAX UNITS
mfurukawa 0:4656a133ed1a 29 MAGNETOMETER SENSITIVITY
mfurukawa 0:4656a133ed1a 30 Full-Scale Range ±4800 µT
mfurukawa 0:4656a133ed1a 31 ADC Word Length 14 bits
mfurukawa 0:4656a133ed1a 32 Sensitivity Scale Factor 0.6 µT / LSB
mfurukawa 0:4656a133ed1a 33 ZERO-FIELD OUTPUT
mfurukawa 0:4656a133ed1a 34 Initial Calibration Tolerance ±500 LSB
mfurukawa 0:4656a133ed1a 35 */
mfurukawa 0:4656a133ed1a 36
mfurukawa 0:4656a133ed1a 37
mfurukawa 0:4656a133ed1a 38 #include "mbed.h"
mfurukawa 0:4656a133ed1a 39 #include "MPU9250.h"
mfurukawa 0:4656a133ed1a 40
mfurukawa 0:4656a133ed1a 41 /* MPU9250 Library
mfurukawa 0:4656a133ed1a 42 *
mfurukawa 0:4656a133ed1a 43 * https://developer.mbed.org/users/kylongmu/code/MPU9250_SPI_Test/file/5839d1b118bc/main.cpp
mfurukawa 0:4656a133ed1a 44 *
mfurukawa 0:4656a133ed1a 45 * MOSI (Master Out Slave In) p5
mfurukawa 0:4656a133ed1a 46 * MISO (Master In Slave Out p6
mfurukawa 0:4656a133ed1a 47 * SCK (Serial Clock) p7
mfurukawa 0:4656a133ed1a 48 * ~CS (Chip Select) p8 -> p30
mfurukawa 0:4656a133ed1a 49 */
mfurukawa 0:4656a133ed1a 50
mfurukawa 0:4656a133ed1a 51 // define serial objects
mfurukawa 0:4656a133ed1a 52 Serial pc(USBTX, USBRX);
mfurukawa 0:4656a133ed1a 53
mfurukawa 0:4656a133ed1a 54 Ticker ticker;
mfurukawa 0:4656a133ed1a 55 Timer timer;
mfurukawa 0:4656a133ed1a 56
mfurukawa 0:4656a133ed1a 57 #define SampleFreq 20 // [Hz]
mfurukawa 0:4656a133ed1a 58 #define nCh 4 // number of ch
mfurukawa 0:4656a133ed1a 59 #define baudRate 921600 //921600 / 115200
mfurukawa 0:4656a133ed1a 60
mfurukawa 0:4656a133ed1a 61 unsigned int counter = 0;
mfurukawa 0:4656a133ed1a 62 unsigned int usCycle = 1000000/SampleFreq ;
mfurukawa 0:4656a133ed1a 63
mfurukawa 0:4656a133ed1a 64 int errFlag = 0;
mfurukawa 0:4656a133ed1a 65
mfurukawa 0:4656a133ed1a 66 //define the mpu9250 object
mfurukawa 0:4656a133ed1a 67 mpu9250_spi *imu[nCh];
mfurukawa 0:4656a133ed1a 68
mfurukawa 0:4656a133ed1a 69 // define SPI object for imu objects
mfurukawa 0:4656a133ed1a 70 SPI spi1(p5, p6, p7);
mfurukawa 0:4656a133ed1a 71 SPI spi2(p11, p12, p13);
mfurukawa 0:4656a133ed1a 72
mfurukawa 0:4656a133ed1a 73 void init(void)
mfurukawa 0:4656a133ed1a 74 {
mfurukawa 0:4656a133ed1a 75 pc.baud(baudRate);
mfurukawa 0:4656a133ed1a 76
mfurukawa 0:4656a133ed1a 77 printf("\nrev Feb 1, 2021 for Magnetrometer by Masahiro Furukawa\n\n");
mfurukawa 0:4656a133ed1a 78
mfurukawa 0:4656a133ed1a 79 imu[3] = new mpu9250_spi(spi2, p21);
mfurukawa 0:4656a133ed1a 80 imu[2] = new mpu9250_spi(spi2, p23);
mfurukawa 0:4656a133ed1a 81 imu[1] = new mpu9250_spi(spi1, p29);
mfurukawa 0:4656a133ed1a 82 imu[0] = new mpu9250_spi(spi1, p30);
mfurukawa 0:4656a133ed1a 83 for(int i=0; i<nCh; i++) {
mfurukawa 0:4656a133ed1a 84
mfurukawa 0:4656a133ed1a 85 imu[0]->deselect();
mfurukawa 0:4656a133ed1a 86 imu[1]->deselect();
mfurukawa 0:4656a133ed1a 87 imu[2]->deselect();
mfurukawa 0:4656a133ed1a 88 imu[3]->deselect();
mfurukawa 0:4656a133ed1a 89
mfurukawa 0:4656a133ed1a 90 imu[i]->select();
mfurukawa 0:4656a133ed1a 91
mfurukawa 0:4656a133ed1a 92 //INIT the mpu9250
mfurukawa 0:4656a133ed1a 93 //if(imu[i]->init(1,BITS_DLPF_CFG_188HZ))
mfurukawa 0:4656a133ed1a 94 //if(imu[i]->init(1,BITS_DLPF_CFG_5HZ))
mfurukawa 0:4656a133ed1a 95 if(imu[i]->init(1,BITS_DLPF_CFG_256HZ_NOLPF2)) {
mfurukawa 0:4656a133ed1a 96 printf("\nCH %d\n\nCouldn't initialize MPU9250 via SPI!", i+1);
mfurukawa 0:4656a133ed1a 97 wait(90);
mfurukawa 0:4656a133ed1a 98 }
mfurukawa 0:4656a133ed1a 99
mfurukawa 0:4656a133ed1a 100 //output the I2C address to know if SPI is working, it should be 104
mfurukawa 0:4656a133ed1a 101 printf("\nCH %d\nWHOAMI = 0x%2x\n",i+1, imu[i]->whoami());
mfurukawa 0:4656a133ed1a 102
mfurukawa 0:4656a133ed1a 103 if(imu[i]->whoami() != 0x71) {
mfurukawa 0:4656a133ed1a 104 printf(" *** ERROR *** acc and gyro sensor does not respond correctly!\n");
mfurukawa 0:4656a133ed1a 105 errFlag |= 0x01<<(i*2);
mfurukawa 0:4656a133ed1a 106 continue;
mfurukawa 0:4656a133ed1a 107 }
mfurukawa 0:4656a133ed1a 108
mfurukawa 0:4656a133ed1a 109 printf("Gyro_scale = %u[DPS]\n",imu[i]->set_gyro_scale(BITS_FS_500DPS)); //Set 500DPS scale range for gyros //0706 wada 500to2000
mfurukawa 0:4656a133ed1a 110 wait_ms(20);
mfurukawa 0:4656a133ed1a 111
mfurukawa 0:4656a133ed1a 112 printf("Acc_scale = %u[G]\n",imu[i]->set_acc_scale(BITS_FS_4G)); //Set 4G scale range for accs //0706 wada 4to16
mfurukawa 0:4656a133ed1a 113 wait_ms(20);
mfurukawa 0:4656a133ed1a 114
mfurukawa 0:4656a133ed1a 115 printf("AK8963 WHIAM = 0x%2x\n",imu[i]->AK8963_whoami());
mfurukawa 0:4656a133ed1a 116
mfurukawa 0:4656a133ed1a 117 if(imu[i]->AK8963_whoami() != 0x48) {
mfurukawa 0:4656a133ed1a 118 printf(" *** ERROR *** magnetrometer does not respond correctly!\n");
mfurukawa 0:4656a133ed1a 119 errFlag |= 0x02<<(i*2);
mfurukawa 0:4656a133ed1a 120 continue;
mfurukawa 0:4656a133ed1a 121 }
mfurukawa 0:4656a133ed1a 122
mfurukawa 0:4656a133ed1a 123 imu[i]->AK8963_calib_Magnetometer();
mfurukawa 0:4656a133ed1a 124 wait_ms(100);
mfurukawa 0:4656a133ed1a 125 printf("Calibrated Magnetrometer\n");
mfurukawa 0:4656a133ed1a 126 }
mfurukawa 0:4656a133ed1a 127
mfurukawa 0:4656a133ed1a 128
mfurukawa 0:4656a133ed1a 129 printf("\nHit Key [s] to start. Hit Key [r] to finish.\n");
mfurukawa 0:4656a133ed1a 130 }
mfurukawa 0:4656a133ed1a 131
mfurukawa 0:4656a133ed1a 132 void eventFunc(void)
mfurukawa 0:4656a133ed1a 133 {
mfurukawa 0:4656a133ed1a 134 // limitation on sending bytes at 921600bps - 92bits(under 100us/sample)
mfurukawa 0:4656a133ed1a 135 // requirement : 1kHz sampling
mfurukawa 0:4656a133ed1a 136 // = 921.5 bits/sample
mfurukawa 0:4656a133ed1a 137 // = 115.1 bytes/sample
mfurukawa 0:4656a133ed1a 138 // = 50 bytes/axis (2byte/axis)
mfurukawa 0:4656a133ed1a 139
mfurukawa 0:4656a133ed1a 140 // 2 byte * 6 axes * 4 ch = 48 bytes/sample
mfurukawa 0:4656a133ed1a 141
mfurukawa 0:4656a133ed1a 142
mfurukawa 0:4656a133ed1a 143 imu[0]->select();
mfurukawa 0:4656a133ed1a 144 imu[1]->deselect();
mfurukawa 0:4656a133ed1a 145 imu[2]->select();
mfurukawa 0:4656a133ed1a 146 imu[3]->deselect();
mfurukawa 0:4656a133ed1a 147
mfurukawa 0:4656a133ed1a 148 imu[0]->AK8963_read_Magnetometer();
mfurukawa 0:4656a133ed1a 149 imu[2]->AK8963_read_Magnetometer();
mfurukawa 0:4656a133ed1a 150
mfurukawa 0:4656a133ed1a 151 imu[0]->deselect();
mfurukawa 0:4656a133ed1a 152 imu[1]->select();
mfurukawa 0:4656a133ed1a 153 imu[2]->deselect();
mfurukawa 0:4656a133ed1a 154 imu[3]->select();
mfurukawa 0:4656a133ed1a 155
mfurukawa 0:4656a133ed1a 156 imu[1]->AK8963_read_Magnetometer();
mfurukawa 0:4656a133ed1a 157 imu[3]->AK8963_read_Magnetometer();
mfurukawa 0:4656a133ed1a 158
mfurukawa 0:4656a133ed1a 159 for(int i=0; i<2; i++) {
mfurukawa 0:4656a133ed1a 160 for(int j=0; j<3; j++) printf("%1.3f, ",imu[i]->Magnetometer[j] / 1000.0f);
mfurukawa 0:4656a133ed1a 161 }
mfurukawa 0:4656a133ed1a 162 printf("[mT]");
mfurukawa 0:4656a133ed1a 163
mfurukawa 0:4656a133ed1a 164 putc(13, stdout); //0x0d CR（復帰）
mfurukawa 0:4656a133ed1a 165 putc(10, stdout); //0x0a LF（改行）
mfurukawa 0:4656a133ed1a 166 // printf("\r\n");
mfurukawa 0:4656a133ed1a 167
mfurukawa 0:4656a133ed1a 168 }
mfurukawa 0:4656a133ed1a 169
mfurukawa 0:4656a133ed1a 170 int main()
mfurukawa 0:4656a133ed1a 171 {
mfurukawa 0:4656a133ed1a 172 // make instances and check sensors
mfurukawa 0:4656a133ed1a 173 init();
mfurukawa 0:4656a133ed1a 174
mfurukawa 0:4656a133ed1a 175 char c;
mfurukawa 0:4656a133ed1a 176
mfurukawa 0:4656a133ed1a 177 while(1) {
mfurukawa 0:4656a133ed1a 178 if(pc.readable()) {
mfurukawa 0:4656a133ed1a 179 c = pc.getc();
mfurukawa 0:4656a133ed1a 180
mfurukawa 0:4656a133ed1a 181 if(c == 'r') {
mfurukawa 0:4656a133ed1a 182 ticker.detach();
mfurukawa 0:4656a133ed1a 183 } else if(c == 's') {
mfurukawa 0:4656a133ed1a 184 ticker.attach_us(eventFunc, 1000000.0f/SampleFreq);
mfurukawa 0:4656a133ed1a 185 }
mfurukawa 0:4656a133ed1a 186 }
mfurukawa 0:4656a133ed1a 187 }
mfurukawa 0:4656a133ed1a 188 }