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
- Branch:
- MPU-9250-MagSensServo
- Revision:
- 6:493df7718ecb
- Parent:
- 5:521f1c79123d
- Child:
- 7:a711da20a262
diff -r 521f1c79123d -r 493df7718ecb main.cpp --- a/main.cpp Tue Feb 02 15:04:04 2021 +0000 +++ b/main.cpp Tue Feb 02 15:39:02 2021 +0000 @@ -56,6 +56,7 @@ // define serial objects Serial pc(USBTX, USBRX); +long int smpl_cnt = 0; Ticker ticker; Timer timer; @@ -87,6 +88,8 @@ #define ASCII_MODE 1 int send_mode = BINARY_MODE; +#define CSV_TITLE_COLMUN "smpl_cnt,time[sec],accX,accY,accZ,gyroX,gyroY,gyroZ,magX,magY,magZ,accX,accY,accZ,gyroX,gyroY,gyroZ,magX,magY,magZ,\r\n" + void servo_test(void) { @@ -124,7 +127,7 @@ 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; } @@ -140,15 +143,15 @@ 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"); @@ -173,13 +176,21 @@ 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 @@ -188,11 +199,6 @@ // 2 byte * 6 axes * 4 ch = 48 bytes/sample - imu[0]->select(); - imu[1]->deselect(); - imu[2]->select(); - imu[3]->deselect(); - // imu[0]->read_acc(); // imu[0]->read_rot(); // imu[0]->AK8963_read_Magnetometer(); @@ -200,10 +206,11 @@ // 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(); @@ -220,21 +227,23 @@ switch(send_mode) { case ASCII_MODE: - + printf("%ld,",smpl_cnt++); + printf("%1.3f,",t_ms/1000.0f); for(int i=0; i<3; i+=2) { - for(int j=0; j<3; j++) printf("a:%1.3f,",imu[i]->accelerometer_data[j]); - for(int j=0; j<3; j++) printf("g:%1.3f,",imu[i]->gyroscope_data[j]); - for(int j=0; j<3; j++) printf("m:%1.3f,",imu[i]->Magnetometer[j] / 1000.0f); + 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: - 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); - } + // 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; } @@ -256,20 +265,35 @@ 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': + smpl_cnt = 0; + timer.reset(); + timer.start(); send_mode = ASCII_MODE; - ticker.attach_us(eventFunc, 1000000.0f/SampleFreq); + ticker.attach_us(eventFunc, 1000000/SampleFreq); + break; + + case 'c': + printf(CSV_TITLE_COLMUN); break; case 's': + smpl_cnt = 0; + timer.reset(); + timer.start(); send_mode = BINARY_MODE; - ticker.attach_us(eventFunc, 1000000.0f/SampleFreq); + ticker.attach_us(eventFunc, 1000000/SampleFreq); break; } }