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
main.cpp
- Committer:
- mfurukawa
- Date:
- 2021-02-01
- Revision:
- 0:4656a133ed1a
- Child:
- 1:3bcd844dd707
File content as of revision 0:4656a133ed1a:
/** * 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); } } } }