Masahiro Furukawa
2Chx3dof Magnetrometer supported
Revision 0:4656a133ed1a, committed 2021-02-01
- Comitter:
- mfurukawa
- Date:
- Mon Feb 01 17:11:03 2021 +0000
- Commit message:
- initial commit;
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/MPU9250_SPI/MPU9250.cpp Mon Feb 01 17:11:03 2021 +0000
@@ -0,0 +1,403 @@
+/*CODED by Qiyong Mu on 21/06/2014
+kylongmu@msn.com
+
+revised by Masahiro Furukawa
+m.furukawa@ist.osaka-u.ac.jp 10/05/2018
+*/
+
+#include <mbed.h>
+#include "MPU9250.h"
+
+mpu9250_spi::mpu9250_spi(SPI& _spi, PinName _cs) : spi(_spi), cs(_cs) {}
+
+unsigned int mpu9250_spi::WriteReg( uint8_t WriteAddr, uint8_t WriteData )
+{
+ unsigned int temp_val;
+ select();
+ spi.write(WriteAddr);
+ temp_val=spi.write(WriteData);
+ deselect();
+ wait_us(50);
+ return temp_val;
+}
+unsigned int mpu9250_spi::ReadReg( uint8_t WriteAddr, uint8_t WriteData )
+{
+ return WriteReg(WriteAddr | READ_FLAG,WriteData);
+}
+void mpu9250_spi::ReadRegs( uint8_t ReadAddr, uint8_t *ReadBuf, unsigned int Bytes )
+{
+ unsigned int i = 0;
+
+ select();
+ spi.write(ReadAddr | READ_FLAG);
+ for(i=0; i<Bytes; i++)
+ ReadBuf[i] = spi.write(0x00);
+ deselect();
+ wait_us(50);
+}
+
+/*-----------------------------------------------------------------------------------------------
+ INITIALIZATION
+usage: call this function at startup, giving the sample rate divider (raging from 0 to 255) and
+low pass filter value; suitable values are:
+BITS_DLPF_CFG_256HZ_NOLPF2
+BITS_DLPF_CFG_188HZ
+BITS_DLPF_CFG_98HZ
+BITS_DLPF_CFG_42HZ
+BITS_DLPF_CFG_20HZ
+BITS_DLPF_CFG_10HZ
+BITS_DLPF_CFG_5HZ
+BITS_DLPF_CFG_2100HZ_NOLPF
+returns 1 if an error occurred
+-----------------------------------------------------------------------------------------------*/
+#define MPU_InitRegNum 17
+
+bool mpu9250_spi::init(int sample_rate_div,int low_pass_filter){
+ uint8_t i = 0;
+ uint8_t MPU_Init_Data[MPU_InitRegNum][2] = {
+ {0x80, MPUREG_PWR_MGMT_1}, // Reset Device
+ {0x01, MPUREG_PWR_MGMT_1}, // Clock Source
+ {0x00, MPUREG_PWR_MGMT_2}, // Enable Acc & Gyro
+ //{low_pass_filter | 0x40, MPUREG_CONFIG}, // [FIFO enabled] Use DLPF set Gyroscope bandwidth 184Hz, temperature bandwidth 188Hz
+
+ //{BITS_DLPF_CFG_256HZ_NOLPF2 , MPUREG_CONFIG}, // no DLPF
+ //{BITS_DLPF_CFG_188HZ , MPUREG_CONFIG}, // Use DLPF set Gyroscope bandwidth 184Hz, temperature bandwidth 188Hz
+ {BITS_DLPF_CFG_5HZ , MPUREG_CONFIG}, // Use DLPF set Gyroscope bandwidth 5Hz, temperature bandwidth 5Hz
+
+ {0x00, MPUREG_GYRO_CONFIG}, // +-250dps = 0x00* +-2000dps = 0x18
+ {0x00, MPUREG_ACCEL_CONFIG}, // +-2G = 0x00* +-4G = 0x08 +-16G = 0x18
+
+ //{0x09, MPUREG_ACCEL_CONFIG_2}, // [ERROR] in accel_fchoice_b. (old comment->)Set Acc Data Rates, Enable Acc LPF , Bandwidth 184Hz
+ //{0x06, MPUREG_ACCEL_CONFIG_2}, // Set Acc Data Rates, Enable Acc LPF , Bandwidth 5Hz
+ //{0x02, MPUREG_ACCEL_CONFIG_2}, // Set Acc Data Rates, Enable Acc LPF , Bandwidth 99Hz
+ {0x01, MPUREG_ACCEL_CONFIG_2}, // Set Acc Data Rates, Enable Acc LPF , Bandwidth 218Hz
+
+ {0x00, MPUREG_INT_PIN_CFG}, // Masahiro Furukawa // NO INTERRUPT Aug 87, 2018
+ //{0x40, MPUREG_I2C_MST_CTRL}, // I2C Speed 348 kHz
+ //{0x20, MPUREG_USER_CTRL}, // Enable AUX
+ {0x20, MPUREG_USER_CTRL}, // I2C Master mode
+ {0x0D, MPUREG_I2C_MST_CTRL}, // I2C configuration multi-master IIC 400KHz
+
+ {AK8963_I2C_ADDR, MPUREG_I2C_SLV0_ADDR}, //Set the I2C slave addres of AK8963 and set for write.
+ //{0x09, MPUREG_I2C_SLV4_CTRL},
+ //{0x81, MPUREG_I2C_MST_DELAY_CTRL}, //Enable I2C delay
+
+ {AK8963_CNTL2, MPUREG_I2C_SLV0_REG}, //I2C slave 0 register address from where to begin data transfer
+ {0x01, MPUREG_I2C_SLV0_DO}, // Reset AK8963
+ {0x81, MPUREG_I2C_SLV0_CTRL}, //Enable I2C and set 1 byte
+
+ {AK8963_CNTL1, MPUREG_I2C_SLV0_REG}, //I2C slave 0 register address from where to begin data transfer
+ {0x12, MPUREG_I2C_SLV0_DO}, // Register value to continuous measurement in 16bit
+ {0x81, MPUREG_I2C_SLV0_CTRL} //Enable I2C and set 1 byte
+
+ };
+ spi.format(8,0);
+ //spi.frequency(1000000);
+ spi.frequency(1000000);
+
+ for(i=0; i<MPU_InitRegNum; i++) {
+ WriteReg(MPU_Init_Data[i][1], MPU_Init_Data[i][0]);
+ wait(0.001); //I2C must slow down the write speed, otherwise it won't work
+ }
+
+ //AK8963_calib_Magnetometer(); //Can't load this function here , strange problem?
+ return 0;
+}
+/*-----------------------------------------------------------------------------------------------
+ ACCELEROMETER SCALE
+usage: call this function at startup, after initialization, to set the right range for the
+accelerometers. Suitable ranges are:
+BITS_FS_2G
+BITS_FS_4G
+BITS_FS_8G
+BITS_FS_16G
+returns the range set (2,4,8 or 16)
+-----------------------------------------------------------------------------------------------*/
+unsigned int mpu9250_spi::set_acc_scale(int scale){
+ unsigned int temp_scale;
+ WriteReg(MPUREG_ACCEL_CONFIG, scale);
+
+ switch (scale){
+ case BITS_FS_2G:
+ acc_divider=16384;
+ break;
+ case BITS_FS_4G:
+ acc_divider=8192;
+ break;
+ case BITS_FS_8G:
+ acc_divider=4096;
+ break;
+ case BITS_FS_16G:
+ acc_divider=2048;
+ break;
+ }
+ temp_scale=WriteReg(MPUREG_ACCEL_CONFIG|READ_FLAG, 0x00);
+
+ switch (temp_scale){
+ case BITS_FS_2G:
+ temp_scale=2;
+ break;
+ case BITS_FS_4G:
+ temp_scale=4;
+ break;
+ case BITS_FS_8G:
+ temp_scale=8;
+ break;
+ case BITS_FS_16G:
+ temp_scale=16;
+ break;
+ }
+ return temp_scale;
+}
+
+
+/*-----------------------------------------------------------------------------------------------
+ GYROSCOPE SCALE
+usage: call this function at startup, after initialization, to set the right range for the
+gyroscopes. Suitable ranges are:
+BITS_FS_250DPS
+BITS_FS_500DPS
+BITS_FS_1000DPS
+BITS_FS_2000DPS
+returns the range set (250,500,1000 or 2000)
+-----------------------------------------------------------------------------------------------*/
+unsigned int mpu9250_spi::set_gyro_scale(int scale){
+ unsigned int temp_scale;
+ WriteReg(MPUREG_GYRO_CONFIG, scale);
+ switch (scale){
+ case BITS_FS_250DPS:
+ gyro_divider=131;
+ break;
+ case BITS_FS_500DPS:
+ gyro_divider=65.5;
+ break;
+ case BITS_FS_1000DPS:
+ gyro_divider=32.8;
+ break;
+ case BITS_FS_2000DPS:
+ gyro_divider=16.4;
+ break;
+ }
+ temp_scale=WriteReg(MPUREG_GYRO_CONFIG|READ_FLAG, 0x00);
+ switch (temp_scale){
+ case BITS_FS_250DPS:
+ temp_scale=250;
+ break;
+ case BITS_FS_500DPS:
+ temp_scale=500;
+ break;
+ case BITS_FS_1000DPS:
+ temp_scale=1000;
+ break;
+ case BITS_FS_2000DPS:
+ temp_scale=2000;
+ break;
+ }
+ return temp_scale;
+}
+
+
+/*-----------------------------------------------------------------------------------------------
+ WHO AM I?
+usage: call this function to know if SPI is working correctly. It checks the I2C address of the
+mpu9250 which should be 104 when in SPI mode.
+returns the I2C address (104)
+-----------------------------------------------------------------------------------------------*/
+unsigned int mpu9250_spi::whoami(){
+ unsigned int response;
+ response=WriteReg(MPUREG_WHOAMI|READ_FLAG, 0x00);
+ return response;
+}
+
+
+/*-----------------------------------------------------------------------------------------------
+ READ ACCELEROMETER
+usage: call this function to read accelerometer data. Axis represents selected axis:
+0 -> X axis
+1 -> Y axis
+2 -> Z axis
+-----------------------------------------------------------------------------------------------*/
+void mpu9250_spi::read_acc()
+{
+ //int16_t bit_data;
+// float data;
+// int i;
+ ReadRegs(MPUREG_ACCEL_XOUT_H,accelerometer_response,6);
+// for(i=0; i<3; i++) {
+// bit_data=((int16_t)accelerometer_response[i*2]<<8)|accelerometer_response[i*2+1];
+// data=(float)bit_data;
+// accelerometer_data[i]=data/acc_divider;
+//
+// }
+
+}
+
+/*-----------------------------------------------------------------------------------------------
+ READ GYROSCOPE
+usage: call this function to read gyroscope data. Axis represents selected axis:
+0 -> X axis
+1 -> Y axis
+2 -> Z axis
+-----------------------------------------------------------------------------------------------*/
+void mpu9250_spi::read_rot()
+{
+// int16_t bit_data;
+// float data;
+// int i;
+ ReadRegs(MPUREG_GYRO_XOUT_H,gyroscope_response,6);
+// for(i=0; i<3; i++) {
+// bit_data=((int16_t)gyroscope_response[i*2]<<8)|gyroscope_response[i*2+1];
+// data=(float)bit_data;
+// gyroscope_data[i]=data/gyro_divider;
+// }
+
+}
+
+/*-----------------------------------------------------------------------------------------------
+ READ TEMPERATURE
+usage: call this function to read temperature data.
+returns the value in °C
+-----------------------------------------------------------------------------------------------*/
+void mpu9250_spi::read_temp(){
+ uint8_t response[2];
+ int16_t bit_data;
+ float data;
+ ReadRegs(MPUREG_TEMP_OUT_H,response,2);
+
+ bit_data=((int16_t)response[0]<<8)|response[1];
+ data=(float)bit_data;
+ Temperature=(data/340)+36.53;
+ deselect();
+}
+
+/*-----------------------------------------------------------------------------------------------
+ READ ACCELEROMETER CALIBRATION
+usage: call this function to read accelerometer data. Axis represents selected axis:
+0 -> X axis
+1 -> Y axis
+2 -> Z axis
+returns Factory Trim value
+-----------------------------------------------------------------------------------------------*/
+void mpu9250_spi::calib_acc()
+{
+ uint8_t response[4];
+ int temp_scale;
+ //READ CURRENT ACC SCALE
+ temp_scale=WriteReg(MPUREG_ACCEL_CONFIG|READ_FLAG, 0x00);
+ set_acc_scale(BITS_FS_8G);
+ //ENABLE SELF TEST need modify
+ //temp_scale=WriteReg(MPUREG_ACCEL_CONFIG, 0x80>>axis);
+
+ ReadRegs(MPUREG_SELF_TEST_X,response,4);
+ calib_data[0]=((response[0]&11100000)>>3)|((response[3]&00110000)>>4);
+ calib_data[1]=((response[1]&11100000)>>3)|((response[3]&00001100)>>2);
+ calib_data[2]=((response[2]&11100000)>>3)|((response[3]&00000011));
+
+ set_acc_scale(temp_scale);
+}
+uint8_t mpu9250_spi::AK8963_whoami(){
+ uint8_t response;
+ WriteReg(MPUREG_I2C_SLV0_ADDR,AK8963_I2C_ADDR|READ_FLAG); //Set the I2C slave addres of AK8963 and set for read.
+ WriteReg(MPUREG_I2C_SLV0_REG, AK8963_WIA); //I2C slave 0 register address from where to begin data transfer
+ WriteReg(MPUREG_I2C_SLV0_CTRL, 0x81); //Read 1 byte from the magnetometer
+
+ //WriteReg(MPUREG_I2C_SLV0_CTRL, 0x81); //Enable I2C and set bytes
+ wait(0.01);
+ response=WriteReg(MPUREG_EXT_SENS_DATA_00|READ_FLAG, 0x00); //Read I2C
+ //ReadRegs(MPUREG_EXT_SENS_DATA_00,response,1);
+ //response=WriteReg(MPUREG_I2C_SLV0_DO, 0x00); //Read I2C
+
+ return response;
+}
+void mpu9250_spi::AK8963_calib_Magnetometer(){
+ uint8_t response[3];
+ float data;
+ int i;
+
+ WriteReg(MPUREG_I2C_SLV0_ADDR,AK8963_I2C_ADDR|READ_FLAG); //Set the I2C slave addres of AK8963 and set for read.
+ WriteReg(MPUREG_I2C_SLV0_REG, AK8963_ASAX); //I2C slave 0 register address from where to begin data transfer
+ WriteReg(MPUREG_I2C_SLV0_CTRL, 0x83); //Read 3 bytes from the magnetometer
+
+ //WriteReg(MPUREG_I2C_SLV0_CTRL, 0x81); //Enable I2C and set bytes
+ wait(0.001);
+ //response[0]=WriteReg(MPUREG_EXT_SENS_DATA_01|READ_FLAG, 0x00); //Read I2C
+ ReadRegs(MPUREG_EXT_SENS_DATA_00,response,3);
+
+ //response=WriteReg(MPUREG_I2C_SLV0_DO, 0x00); //Read I2C
+ for(i=0; i<3; i++) {
+ data=response[i];
+ Magnetometer_ASA[i]=((data-128)/256+1)*Magnetometer_Sensitivity_Scale_Factor;
+
+ }
+}
+void mpu9250_spi::AK8963_read_Magnetometer(){
+ uint8_t response[7];
+ int16_t bit_data;
+ float data;
+ int i;
+
+ WriteReg(MPUREG_I2C_SLV0_ADDR,AK8963_I2C_ADDR|READ_FLAG); //Set the I2C slave addres of AK8963 and set for read.
+ WriteReg(MPUREG_I2C_SLV0_REG, AK8963_HXL); //I2C slave 0 register address from where to begin data transfer
+ WriteReg(MPUREG_I2C_SLV0_CTRL, 0x87); //Read 6 bytes from the magnetometer
+
+ wait(0.001);
+ ReadRegs(MPUREG_EXT_SENS_DATA_00,response,7);
+ //must start your read from AK8963A register 0x03 and read seven bytes so that upon read of ST2 register 0x09 the AK8963A will unlatch the data registers for the next measurement.
+ for(i=0; i<3; i++) {
+ bit_data=((int16_t)response[i*2+1]<<8)|response[i*2];
+ data=(float)bit_data;
+ Magnetometer[i]=data*Magnetometer_ASA[i];
+ }
+}
+void mpu9250_spi::read_all(){
+ uint8_t response[21];
+ int16_t bit_data;
+ float data;
+ int i;
+
+ //Send I2C command at first
+ WriteReg(MPUREG_I2C_SLV0_ADDR,AK8963_I2C_ADDR|READ_FLAG); //Set the I2C slave addres of AK8963 and set for read.
+ WriteReg(MPUREG_I2C_SLV0_REG, AK8963_HXL); //I2C slave 0 register address from where to begin data transfer
+ WriteReg(MPUREG_I2C_SLV0_CTRL, 0x87); //Read 7 bytes from the magnetometer
+ //must start your read from AK8963A register 0x03 and read seven bytes so that upon read of ST2 register 0x09 the AK8963A will unlatch the data registers for the next measurement.
+
+ wait(0.001);
+ ReadRegs(MPUREG_ACCEL_XOUT_H,response,21);
+ //Get accelerometer value
+ for(i=0; i<3; i++) {
+ bit_data=((int16_t)response[i*2]<<8)|response[i*2+1];
+ data=(float)bit_data;
+ accelerometer_data[i]=data/acc_divider;
+ }
+ //Get temperature
+ bit_data=((int16_t)response[i*2]<<8)|response[i*2+1];
+ data=(float)bit_data;
+ Temperature=((data-21)/333.87)+21;
+ //Get gyroscop value
+ for(i=4; i<7; i++) {
+ bit_data=((int16_t)response[i*2]<<8)|response[i*2+1];
+ data=(float)bit_data;
+ gyroscope_data[i-4]=data/gyro_divider;
+ //printf("%+4.2f \n",data);
+ }
+ //Get Magnetometer value
+ for(i=7; i<10; i++) {
+ bit_data=((int16_t)response[i*2+1]<<8)|response[i*2];
+ data=(float)bit_data;
+ Magnetometer[i-7]=data*Magnetometer_ASA[i-7];
+
+ }
+}
+
+/*-----------------------------------------------------------------------------------------------
+ SPI SELECT AND DESELECT
+usage: enable and disable mpu9250 communication bus
+-----------------------------------------------------------------------------------------------*/
+void mpu9250_spi::select() {
+ //Set CS low to start transmission (interrupts conversion)
+ cs = 0;
+}
+void mpu9250_spi::deselect() {
+ //Set CS high to stop transmission (restarts conversion)
+ cs = 1;
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/MPU9250_SPI/MPU9250.h Mon Feb 01 17:11:03 2021 +0000
@@ -0,0 +1,267 @@
+/*CODED by Qiyong Mu on 21/06/2014
+kylongmu@msn.com
+
+revised by Masahiro Furukawa
+m.furukawa@ist.osaka-u.ac.jp 10/05/2018
+*/
+
+
+#ifndef mpu9250_h
+#define mpu9250_h
+#include "mbed.h"
+
+
+class mpu9250_spi
+{
+ SPI& spi;
+ DigitalOut cs;
+
+public:
+ mpu9250_spi(SPI& _spi, PinName _cs);
+ unsigned int WriteReg( uint8_t WriteAddr, uint8_t WriteData );
+ unsigned int ReadReg( uint8_t WriteAddr, uint8_t WriteData );
+ void ReadRegs( uint8_t ReadAddr, uint8_t *ReadBuf, unsigned int Bytes );
+
+ bool init(int sample_rate_div,int low_pass_filter);
+ void read_temp();
+ void read_acc();
+ void read_rot();
+ unsigned int set_gyro_scale(int scale);
+ unsigned int set_acc_scale(int scale);
+ void calib_acc();
+ void AK8963_calib_Magnetometer();
+ void select();
+ void deselect();
+ unsigned int whoami();
+ uint8_t AK8963_whoami();
+ void AK8963_read_Magnetometer();
+ void read_all();
+
+
+
+ float acc_divider;
+ float gyro_divider;
+
+ int calib_data[3];
+ float Magnetometer_ASA[3];
+
+ uint8_t accelerometer_response[6]; //追加
+ uint8_t gyroscope_response[6]; //追加
+
+ union {
+ float accelerometer_data[3];
+ unsigned long accelerometer_long[3]; // 4 bytes x 3ch
+ };
+ float Temperature;
+ union {
+ float gyroscope_data[3];
+ unsigned long gyroscope_long[3]; // 4 bytes x 3ch
+ };
+ union {
+ float Magnetometer[3];
+ unsigned long Magnetometer_long[3]; // 4 bytes x 3ch
+ };
+
+private:
+ PinName _CS_pin;
+ PinName _SO_pin;
+ PinName _SCK_pin;
+ float _error;
+ };
+
+#endif
+
+
+
+// mpu9250 registers
+#define MPUREG_XG_OFFS_TC 0x00
+#define MPUREG_YG_OFFS_TC 0x01
+#define MPUREG_ZG_OFFS_TC 0x02
+#define MPUREG_X_FINE_GAIN 0x03
+#define MPUREG_Y_FINE_GAIN 0x04
+#define MPUREG_Z_FINE_GAIN 0x05
+#define MPUREG_XA_OFFS_H 0x06
+#define MPUREG_XA_OFFS_L 0x07
+#define MPUREG_YA_OFFS_H 0x08
+#define MPUREG_YA_OFFS_L 0x09
+#define MPUREG_ZA_OFFS_H 0x0A
+#define MPUREG_ZA_OFFS_L 0x0B
+#define MPUREG_PRODUCT_ID 0x0C
+#define MPUREG_SELF_TEST_X 0x0D
+#define MPUREG_SELF_TEST_Y 0x0E
+#define MPUREG_SELF_TEST_Z 0x0F
+#define MPUREG_SELF_TEST_A 0x10
+#define MPUREG_XG_OFFS_USRH 0x13
+#define MPUREG_XG_OFFS_USRL 0x14
+#define MPUREG_YG_OFFS_USRH 0x15
+#define MPUREG_YG_OFFS_USRL 0x16
+#define MPUREG_ZG_OFFS_USRH 0x17
+#define MPUREG_ZG_OFFS_USRL 0x18
+#define MPUREG_SMPLRT_DIV 0x19
+#define MPUREG_CONFIG 0x1A
+#define MPUREG_GYRO_CONFIG 0x1B
+#define MPUREG_ACCEL_CONFIG 0x1C
+#define MPUREG_ACCEL_CONFIG_2 0x1D
+#define MPUREG_LP_ACCEL_ODR 0x1E
+#define MPUREG_MOT_THR 0x1F
+#define MPUREG_FIFO_EN 0x23
+#define MPUREG_I2C_MST_CTRL 0x24
+#define MPUREG_I2C_SLV0_ADDR 0x25
+#define MPUREG_I2C_SLV0_REG 0x26
+#define MPUREG_I2C_SLV0_CTRL 0x27
+#define MPUREG_I2C_SLV1_ADDR 0x28
+#define MPUREG_I2C_SLV1_REG 0x29
+#define MPUREG_I2C_SLV1_CTRL 0x2A
+#define MPUREG_I2C_SLV2_ADDR 0x2B
+#define MPUREG_I2C_SLV2_REG 0x2C
+#define MPUREG_I2C_SLV2_CTRL 0x2D
+#define MPUREG_I2C_SLV3_ADDR 0x2E
+#define MPUREG_I2C_SLV3_REG 0x2F
+#define MPUREG_I2C_SLV3_CTRL 0x30
+#define MPUREG_I2C_SLV4_ADDR 0x31
+#define MPUREG_I2C_SLV4_REG 0x32
+#define MPUREG_I2C_SLV4_DO 0x33
+#define MPUREG_I2C_SLV4_CTRL 0x34
+#define MPUREG_I2C_SLV4_DI 0x35
+#define MPUREG_I2C_MST_STATUS 0x36
+#define MPUREG_INT_PIN_CFG 0x37
+#define MPUREG_INT_ENABLE 0x38
+#define MPUREG_ACCEL_XOUT_H 0x3B
+#define MPUREG_ACCEL_XOUT_L 0x3C
+#define MPUREG_ACCEL_YOUT_H 0x3D
+#define MPUREG_ACCEL_YOUT_L 0x3E
+#define MPUREG_ACCEL_ZOUT_H 0x3F
+#define MPUREG_ACCEL_ZOUT_L 0x40
+#define MPUREG_TEMP_OUT_H 0x41
+#define MPUREG_TEMP_OUT_L 0x42
+#define MPUREG_GYRO_XOUT_H 0x43
+#define MPUREG_GYRO_XOUT_L 0x44
+#define MPUREG_GYRO_YOUT_H 0x45
+#define MPUREG_GYRO_YOUT_L 0x46
+#define MPUREG_GYRO_ZOUT_H 0x47
+#define MPUREG_GYRO_ZOUT_L 0x48
+#define MPUREG_EXT_SENS_DATA_00 0x49
+#define MPUREG_EXT_SENS_DATA_01 0x4A
+#define MPUREG_EXT_SENS_DATA_02 0x4B
+#define MPUREG_EXT_SENS_DATA_03 0x4C
+#define MPUREG_EXT_SENS_DATA_04 0x4D
+#define MPUREG_EXT_SENS_DATA_05 0x4E
+#define MPUREG_EXT_SENS_DATA_06 0x4F
+#define MPUREG_EXT_SENS_DATA_07 0x50
+#define MPUREG_EXT_SENS_DATA_08 0x51
+#define MPUREG_EXT_SENS_DATA_09 0x52
+#define MPUREG_EXT_SENS_DATA_10 0x53
+#define MPUREG_EXT_SENS_DATA_11 0x54
+#define MPUREG_EXT_SENS_DATA_12 0x55
+#define MPUREG_EXT_SENS_DATA_13 0x56
+#define MPUREG_EXT_SENS_DATA_14 0x57
+#define MPUREG_EXT_SENS_DATA_15 0x58
+#define MPUREG_EXT_SENS_DATA_16 0x59
+#define MPUREG_EXT_SENS_DATA_17 0x5A
+#define MPUREG_EXT_SENS_DATA_18 0x5B
+#define MPUREG_EXT_SENS_DATA_19 0x5C
+#define MPUREG_EXT_SENS_DATA_20 0x5D
+#define MPUREG_EXT_SENS_DATA_21 0x5E
+#define MPUREG_EXT_SENS_DATA_22 0x5F
+#define MPUREG_EXT_SENS_DATA_23 0x60
+#define MPUREG_I2C_SLV0_DO 0x63
+#define MPUREG_I2C_SLV1_DO 0x64
+#define MPUREG_I2C_SLV2_DO 0x65
+#define MPUREG_I2C_SLV3_DO 0x66
+#define MPUREG_I2C_MST_DELAY_CTRL 0x67
+#define MPUREG_SIGNAL_PATH_RESET 0x68
+#define MPUREG_MOT_DETECT_CTRL 0x69
+#define MPUREG_USER_CTRL 0x6A
+#define MPUREG_PWR_MGMT_1 0x6B
+#define MPUREG_PWR_MGMT_2 0x6C
+#define MPUREG_BANK_SEL 0x6D
+#define MPUREG_MEM_START_ADDR 0x6E
+#define MPUREG_MEM_R_W 0x6F
+#define MPUREG_DMP_CFG_1 0x70
+#define MPUREG_DMP_CFG_2 0x71
+#define MPUREG_FIFO_COUNTH 0x72
+#define MPUREG_FIFO_COUNTL 0x73
+#define MPUREG_FIFO_R_W 0x74
+#define MPUREG_WHOAMI 0x75
+#define MPUREG_XA_OFFSET_H 0x77
+#define MPUREG_XA_OFFSET_L 0x78
+#define MPUREG_YA_OFFSET_H 0x7A
+#define MPUREG_YA_OFFSET_L 0x7B
+#define MPUREG_ZA_OFFSET_H 0x7D
+#define MPUREG_ZA_OFFSET_L 0x7E
+ /* ---- AK8963 Reg In MPU9250 ----------------------------------------------- */
+
+#define AK8963_I2C_ADDR 0x0c//0x18
+#define AK8963_Device_ID 0x48
+
+// Read-only Reg
+#define AK8963_WIA 0x00
+#define AK8963_INFO 0x01
+#define AK8963_ST1 0x02
+#define AK8963_HXL 0x03
+#define AK8963_HXH 0x04
+#define AK8963_HYL 0x05
+#define AK8963_HYH 0x06
+#define AK8963_HZL 0x07
+#define AK8963_HZH 0x08
+#define AK8963_ST2 0x09
+// Write/Read Reg
+#define AK8963_CNTL1 0x0A
+#define AK8963_CNTL2 0x0B
+#define AK8963_ASTC 0x0C
+#define AK8963_TS1 0x0D
+#define AK8963_TS2 0x0E
+#define AK8963_I2CDIS 0x0F
+// Read-only Reg ( ROM )
+#define AK8963_ASAX 0x10
+#define AK8963_ASAY 0x11
+#define AK8963_ASAZ 0x12
+
+// Configuration bits mpu9250
+#define BIT_SLEEP 0x40
+#define BIT_H_RESET 0x80
+#define BITS_CLKSEL 0x07
+#define MPU_CLK_SEL_PLLGYROX 0x01
+#define MPU_CLK_SEL_PLLGYROZ 0x03
+#define MPU_EXT_SYNC_GYROX 0x02
+#define BITS_FS_250DPS 0x00
+#define BITS_FS_500DPS 0x08
+#define BITS_FS_1000DPS 0x10
+#define BITS_FS_2000DPS 0x18
+#define BITS_FS_2G 0x00
+#define BITS_FS_4G 0x08
+#define BITS_FS_8G 0x10
+#define BITS_FS_16G 0x18
+#define BITS_FS_MASK 0x18
+#define BITS_DLPF_CFG_256HZ_NOLPF2 0x00
+#define BITS_DLPF_CFG_188HZ 0x01
+#define BITS_DLPF_CFG_98HZ 0x02
+#define BITS_DLPF_CFG_42HZ 0x03
+#define BITS_DLPF_CFG_20HZ 0x04
+#define BITS_DLPF_CFG_10HZ 0x05
+#define BITS_DLPF_CFG_5HZ 0x06
+#define BITS_DLPF_CFG_2100HZ_NOLPF 0x07
+#define BITS_DLPF_CFG_MASK 0x07
+#define BIT_INT_ANYRD_2CLEAR 0x10
+#define BIT_RAW_RDY_EN 0x01
+#define BIT_I2C_IF_DIS 0x10
+
+#define READ_FLAG 0x80
+
+/* ---- Sensitivity --------------------------------------------------------- */
+
+#define MPU9250A_2g ((float)0.000061035156f) // 0.000061035156 g/LSB
+#define MPU9250A_4g ((float)0.000122070312f) // 0.000122070312 g/LSB
+#define MPU9250A_8g ((float)0.000244140625f) // 0.000244140625 g/LSB
+#define MPU9250A_16g ((float)0.000488281250f) // 0.000488281250 g/LSB
+
+#define MPU9250G_250dps ((float)0.007633587786f) // 0.007633587786 dps/LSB
+#define MPU9250G_500dps ((float)0.015267175572f) // 0.015267175572 dps/LSB
+#define MPU9250G_1000dps ((float)0.030487804878f) // 0.030487804878 dps/LSB
+#define MPU9250G_2000dps ((float)0.060975609756f) // 0.060975609756 dps/LSB
+
+#define MPU9250M_4800uT ((float)0.6f) // 0.6 uT/LSB
+
+#define MPU9250T_85degC ((float)0.002995177763f) // 0.002995177763 degC/LSB
+
+#define Magnetometer_Sensitivity_Scale_Factor ((float)0.15f)
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Mon Feb 01 17:11:03 2021 +0000
@@ -0,0 +1,188 @@
+/**
+ * Masahiro FURUKAWA - m.furukawa@ist.osaka-u.ac.jp
+ *
+ * Dec 26, 2017
+ * Aug 29, 2018
+ * Dec 17, 2019 @ Digital Low Pass Filter 5Hz -> No LPF
+ @ ACC range 2G -> 4G
+ @ GYRO range 250 -> 500 Degree per second
+ @ Deleted magnet sensor checking
+ @ Set Acc Data Rates, Enable Acc LPF , Bandwidth 218Hz
+ @ Use DLPF set Gyroscope bandwidth 5Hz, temperature bandwidth 5Hz
+ * Feb 1, 2021 @Magnetro Meter Ch1
+ *
+ * MPU9250 9DoF Sensor (Extended to Ch1 ~ Ch4)
+ *
+ **/
+
+/*
+ https://invensense.tdk.com/wp-content/uploads/2015/02/PS-MPU-9250A-01-v1.1.pdf
+
+ 3.3 Magnetometer Specifications
+
+ Typical Operating Circuit of section 4.2,
+ VDD = 2.5V,
+ VDDIO = 2.5V,
+ TA=25°C, unless otherwise noted.
+
+ PARAMETER CONDITIONS MIN TYP MAX UNITS
+ MAGNETOMETER SENSITIVITY
+ Full-Scale Range ±4800 µT
+ ADC Word Length 14 bits
+ Sensitivity Scale Factor 0.6 µT / LSB
+ ZERO-FIELD OUTPUT
+ Initial Calibration Tolerance ±500 LSB
+*/
+
+
+#include "mbed.h"
+#include "MPU9250.h"
+
+/* MPU9250 Library
+ *
+ * https://developer.mbed.org/users/kylongmu/code/MPU9250_SPI_Test/file/5839d1b118bc/main.cpp
+ *
+ * MOSI (Master Out Slave In) p5
+ * MISO (Master In Slave Out p6
+ * SCK (Serial Clock) p7
+ * ~CS (Chip Select) p8 -> p30
+ */
+
+// define serial objects
+Serial pc(USBTX, USBRX);
+
+Ticker ticker;
+Timer timer;
+
+#define SampleFreq 20 // [Hz]
+#define nCh 4 // number of ch
+#define baudRate 921600 //921600 / 115200
+
+unsigned int counter = 0;
+unsigned int usCycle = 1000000/SampleFreq ;
+
+int errFlag = 0;
+
+//define the mpu9250 object
+mpu9250_spi *imu[nCh];
+
+// define SPI object for imu objects
+SPI spi1(p5, p6, p7);
+SPI spi2(p11, p12, p13);
+
+void init(void)
+{
+ pc.baud(baudRate);
+
+ printf("\nrev Feb 1, 2021 for Magnetrometer by Masahiro Furukawa\n\n");
+
+ imu[3] = new mpu9250_spi(spi2, p21);
+ imu[2] = new mpu9250_spi(spi2, p23);
+ imu[1] = new mpu9250_spi(spi1, p29);
+ imu[0] = new mpu9250_spi(spi1, p30);
+ for(int i=0; i<nCh; i++) {
+
+ imu[0]->deselect();
+ imu[1]->deselect();
+ imu[2]->deselect();
+ imu[3]->deselect();
+
+ imu[i]->select();
+
+ //INIT the mpu9250
+ //if(imu[i]->init(1,BITS_DLPF_CFG_188HZ))
+ //if(imu[i]->init(1,BITS_DLPF_CFG_5HZ))
+ if(imu[i]->init(1,BITS_DLPF_CFG_256HZ_NOLPF2)) {
+ printf("\nCH %d\n\nCouldn't initialize MPU9250 via SPI!", i+1);
+ wait(90);
+ }
+
+ //output the I2C address to know if SPI is working, it should be 104
+ printf("\nCH %d\nWHOAMI = 0x%2x\n",i+1, imu[i]->whoami());
+
+ if(imu[i]->whoami() != 0x71) {
+ printf(" *** ERROR *** acc and gyro sensor does not respond correctly!\n");
+ errFlag |= 0x01<<(i*2);
+ continue;
+ }
+
+ printf("Gyro_scale = %u[DPS]\n",imu[i]->set_gyro_scale(BITS_FS_500DPS)); //Set 500DPS scale range for gyros //0706 wada 500to2000
+ wait_ms(20);
+
+ printf("Acc_scale = %u[G]\n",imu[i]->set_acc_scale(BITS_FS_4G)); //Set 4G scale range for accs //0706 wada 4to16
+ wait_ms(20);
+
+ printf("AK8963 WHIAM = 0x%2x\n",imu[i]->AK8963_whoami());
+
+ if(imu[i]->AK8963_whoami() != 0x48) {
+ printf(" *** ERROR *** magnetrometer does not respond correctly!\n");
+ errFlag |= 0x02<<(i*2);
+ continue;
+ }
+
+ imu[i]->AK8963_calib_Magnetometer();
+ wait_ms(100);
+ printf("Calibrated Magnetrometer\n");
+ }
+
+
+ printf("\nHit Key [s] to start. Hit Key [r] to finish.\n");
+}
+
+void eventFunc(void)
+{
+ // limitation on sending bytes at 921600bps - 92bits(under 100us/sample)
+ // requirement : 1kHz sampling
+ // = 921.5 bits/sample
+ // = 115.1 bytes/sample
+ // = 50 bytes/axis (2byte/axis)
+
+ // 2 byte * 6 axes * 4 ch = 48 bytes/sample
+
+
+ imu[0]->select();
+ imu[1]->deselect();
+ imu[2]->select();
+ imu[3]->deselect();
+
+ imu[0]->AK8963_read_Magnetometer();
+ imu[2]->AK8963_read_Magnetometer();
+
+ imu[0]->deselect();
+ imu[1]->select();
+ imu[2]->deselect();
+ imu[3]->select();
+
+ imu[1]->AK8963_read_Magnetometer();
+ imu[3]->AK8963_read_Magnetometer();
+
+ for(int i=0; i<2; i++) {
+ for(int j=0; j<3; j++) printf("%1.3f, ",imu[i]->Magnetometer[j] / 1000.0f);
+ }
+ printf("[mT]");
+
+ putc(13, stdout); //0x0d CR（復帰）
+ putc(10, stdout); //0x0a LF（改行）
+// printf("\r\n");
+
+}
+
+int main()
+{
+ // make instances and check sensors
+ init();
+
+ char c;
+
+ while(1) {
+ if(pc.readable()) {
+ c = pc.getc();
+
+ if(c == 'r') {
+ ticker.detach();
+ } else if(c == 's') {
+ ticker.attach_us(eventFunc, 1000000.0f/SampleFreq);
+ }
+ }
+ }
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Mon Feb 01 17:11:03 2021 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400 \ No newline at end of file