2Chx3dof Magnetrometer supported M-Series Random Sequence Generator Servo Control
Sampling Frequency
Sampling Frequency in main.cpp
#define SampleFreq 200 // [Hz]
Auto Stop Setting
Auto-stop Timer 15sec after
// auto-stop when 15sec after if(smpl_cnt>3000){stop_dump();}
The number of 3000 means Sample Count. The number is given by SampleFreq[Hz] * Auto-Stop Time [sec].
M-Series Random Sequence
M-series Random Update Term in main.cpp
// M-series update flag #define M_TERM 200;
Unit is sample count.
cf.) 200 equals to 200 [samples] which equals to 1 [second] where SampleFreq = 200 [Hz}.
See above.
M-Series Random Servo Control
Diff: main.cpp
- Revision:
- 0:4656a133ed1a
- Child:
- 1:3bcd844dd707
--- /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); + } + } + } +}