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-02
- Branch:
- MPU-9250-MagSensServo
- Revision:
- 8:07c3cb01a5b6
- Parent:
- 7:a711da20a262
- Child:
- 9:b32312aacbba
File content as of revision 8:07c3cb01a5b6:
/** * 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 & Ch3, @ 2000DPS, 2G, DLP 5Hz @ M-Series Random Sequence Added @ Servo Motor PWM (p22), @ Text Dump is prioritized @ LED Flashing to appeal ERROR State * * 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" #include "KST_Servo.h" #include "M-Series.h" #include "error_led_flash.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); long int smpl_cnt = 0; Ticker ticker; Timer timer; #define SampleFreq 200 // [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); // pins already used for SPI Chip selector pin // p21, p23, p29, p30 // Servo Motor Control PwmOut pwm_(p22); // M-Series Random Sequence Mseries m; // M-series update flag int m_flag = 0; #define BINARY_MODE 0 #define ASCII_MODE 1 int send_mode = BINARY_MODE; #define CSV_TITLE_COLUMN "smpl_cnt,time[sec],M_stat,accX,accY,accZ,gyroX,gyroY,gyroZ,magX,magY,magZ,accX,accY,accZ,gyroX,gyroY,gyroZ,magX,magY,magZ,\r\n" void servo_test(void) { while(1) { if(m.update()) pwm_.pulsewidth_us(KST_SERVO_USEC_MIN); else pwm_.pulsewidth_us(KST_SERVO_USEC_90); wait(.5); } } void init_sensor(void) { 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); if(i==0||i==2) LED_flash_error_notice(i); continue; } printf("Gyro_scale = %u[DPS]\n",imu[i]->set_gyro_scale(BITS_FS_2000DPS)); //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_2G)); //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); if(i==0||i==2) LED_flash_error_notice(i); continue; } imu[i]->calib_acc(); wait_ms(100); printf("Calibrated Acc\n"); imu[i]->AK8963_calib_Magnetometer(); wait_ms(100); printf("Calibrated Magnetrometer\n"); } } void init(void) { // servo requires a 20ms period pwm_.period_ms(20); pc.baud(baudRate); printf("\nrev Feb 1, 2021 for Magnetrometer by Masahiro Furukawa\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); init_sensor(); // servo_test(); printf("\nHit Key [t] to start. Hit Key [r] to finish. [s] for binary sending.\n"); // fixed channel usage (only Ch1, Ch3 is connected) imu[0]->select(); // Ch1 imu[1]->deselect(); imu[2]->select(); // Ch3 imu[3]->deselect(); } void eventFunc(void) { int t_ms = timer.read_ms() - 5; // initial call takes 5ms // 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]->read_acc(); // imu[0]->read_rot(); // imu[0]->AK8963_read_Magnetometer(); // // imu[2]->read_acc(); // imu[2]->read_rot(); // imu[2]->AK8963_read_Magnetometer(); // measurement step takes 3ms imu[0]->read_all(); imu[2]->read_all(); // imu[0]->deselect(); // imu[1]->select(); // imu[2]->deselect(); // imu[3]->select(); // imu[1]->read_acc(); // imu[1]->read_rot(); // imu[1]->AK8963_read_Magnetometer(); // imu[3]->read_acc(); // imu[3]->read_rot(); // imu[3]->AK8963_read_Magnetometer(); switch(send_mode) { case ASCII_MODE: printf("%ld,",smpl_cnt++); printf("%1.3f,",t_ms/1000.0f); printf("%d,",m.update()); for(int i=0; i<3; i+=2) { for(int j=0; j<3; j++) printf("%1.3f,",imu[i]->accelerometer_data[j]); for(int j=0; j<3; j++) printf("%1.3f,",imu[i]->gyroscope_data[j]); for(int j=0; j<3; j++) printf("%1.3f,",imu[i]->Magnetometer[j] / 1000.0f); } // printf("[mT]"); break; case BINARY_MODE: // TO BE IMPLEMENTED ... // for(int i=0; i<3; i+=2) { // for(int j=0; j<6; j++) putc(imu[i]->accelerometer_response[j], stdout); // for(int j=0; j<6; j++) putc(imu[i]->gyroscope_response[j], stdout); // } break; } putc(13, stdout); //0x0d CR(復帰) putc(10, stdout); //0x0a LF(改行) } int main() { // make instances and check sensors init(); char c; while(1) { if(pc.readable()) { c = pc.getc(); switch(c) { case 'r': ticker.detach(); timer.stop(); smpl_cnt = 0; break; case 'R': smpl_cnt = 0; init_sensor(); timer.stop(); timer.reset(); break; case 't': printf(CSV_TITLE_COLUMN); smpl_cnt = 0; m.init(); timer.reset(); timer.start(); send_mode = ASCII_MODE; ticker.attach_us(eventFunc, 1000000/SampleFreq); break; case 'c': printf(CSV_TITLE_COLUMN); break; case 's': smpl_cnt = 0; timer.reset(); timer.start(); send_mode = BINARY_MODE; ticker.attach_us(eventFunc, 1000000/SampleFreq); break; } } } }