![](/media/cache/profiles/d741d4ae35aff8fd3674206068e40684.jpg.50x50_q85.jpg)
fork
Dependencies: MPU9250_SPI mbed
Fork of MPU9250_AHRS by
main.cpp
- Committer:
- uribotail
- Date:
- 2016-07-06
- Revision:
- 29:6075f35f472f
- Parent:
- 28:76e2ba7a1ecd
File content as of revision 29:6075f35f472f:
/** * Masahiro FURUKAWA - m.furukawa@ist.osaka-u.ac.jp * * June 17, 2016 * * MPU9250 9DoF Sensor (Extended to Ch1 ~ Ch2) * **/ #include "mbed.h" #include "MPU9250.h" #include "MadgwickAHRS.h" #include "MahonyAHRS.h" //#define DEBUG_putc //Wada /* 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 */ /* Madgwick AHRS Library * * AHRS algorithm is one of hte sensor fusion algorism. * http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/AHRS algorithm is one of hte sensor fusion algorism. * http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/ */ //define the mpu9250 object mpu9250_spi *imu[2]; // define AHRS filters MadgwickAHRS *ahrs[2]; // define serial objects Serial pc(USBTX, USBRX); // define SPI object for imu objects SPI spi(p5, p6, p7); Ticker ticker; float putg[3] = {0}; float putq[4] = {0}; int waittime = 80; void init(void) { pc.baud(115200); //921600 imu[0] = new mpu9250_spi(spi, p8); imu[1] = new mpu9250_spi(spi, p9); ahrs[0] = new MadgwickAHRS(); ahrs[1] = new MadgwickAHRS(); for(int i=0; i<2; i++) { imu[0]->deselect(); imu[1]->deselect(); imu[i]->select(); if(imu[i]->init(1,BITS_DLPF_CFG_188HZ)) { //INIT the mpu9250 printf("\nCH %d\n\nCouldn't initialize MPU9250 via SPI!", i); wait(90); } printf("\nCH %d\nWHOAMI=0x%2x\n",i, imu[i]->whoami()); //output the I2C address to know if SPI is working, it should be 104 printf("Gyro_scale=%u\n",imu[i]->set_gyro_scale(BITS_FS_2000DPS)); //Set 500DPS scale range for gyros //0706 wada 500to2000 printf("Acc_scale=%u\n",imu[i]->set_acc_scale(BITS_FS_16G)); //Set 4G scale range for accs //0706 wada 4to16 printf("AK8963 WHIAM=0x%2x\n",imu[i]->AK8963_whoami()); imu[i]->AK8963_calib_Magnetometer(); wait(0.1); } } // Wada June 17, 2016 void float2byte(float gq, float k)//k = gyro_divider 32.8 or q_divider 32800 { int tmp; float omegaF = gq * k; int16_t omegaI = (int16_t)omegaF; if(omegaI < 0){//1の補数 omegaI = 0xFFFF + omegaI; } for(int i=0;i<4;i++){ tmp = (0xF & (omegaI >> (i*4))); #ifdef DEBUG_putc pc.putc(tmp); #endif wait_us(waittime); } }//float2byte void eventFunc(void) { for(int i=0; i<2; i++) { imu[0]->deselect(); imu[1]->deselect(); imu[i]->select(); imu[i]->read_all(); } // update filters for(int i=0; i<2; i++) { ahrs[i]->update( imu[i]->gyroscope_data[0]*DEGREE2RAD, imu[i]->gyroscope_data[1]*DEGREE2RAD, imu[i]->gyroscope_data[2]*DEGREE2RAD, imu[i]->accelerometer_data[0], imu[i]->accelerometer_data[1], imu[i]->accelerometer_data[2], imu[i]->Magnetometer[0], imu[i]->Magnetometer[1], imu[i]->Magnetometer[2] ); } /* printf("%+4.3f %+4.3f %+4.3f ", imu[1]->Magnetometer[0], imu[1]->Magnetometer[1], imu[1]->Magnetometer[2] ); printf("\n"); */ #ifdef DEBUG_putc pc.putc(0x34); //STX #endif wait_us(waittime); for(int i=0; i<2; i++) { putg[0] = imu[i]->gyroscope_data[0]; putg[1] = imu[i]->gyroscope_data[1]; putg[2] = imu[i]->gyroscope_data[2]; putq[0] = ahrs[i]->q0; putq[1] = ahrs[i]->q1; putq[2] = ahrs[i]->q2; putq[3] = ahrs[i]->q3; /* printf("%+0.3f,%+0.3f,%+0.3f,%+0.3f,%+0.3f,%+0.3f,%+0.3f,", putg[0]*DEGREE2RAD, putg[1]*DEGREE2RAD, putg[2]*DEGREE2RAD, putq[0], putq[1], putq[2], putq[3]); */ float2byte(putg[0],16.4); float2byte(putg[1],16.4); float2byte(putg[2],16.4); float2byte(putq[0],32800); float2byte(putq[1],32800); float2byte(putq[2],32800); float2byte(putq[3],32800); /* //test signal 29,June wada float2byte(-200,32.8); float2byte( 0,32.8); float2byte( 200,32.8); float2byte(-0.2,32800); float2byte( 0,32800); float2byte( 0.4,32800); float2byte( 0.8,32800); */ } #ifdef DEBUG_putc pc.putc(0x12); //ETX #endif //printf("\n"); } int main() { // make instances and check sensors init(); // define callback event ticker.attach_us(eventFunc, 1000000.0f/sampleFreq); while(1) { if(pc.readable()) if(pc.getc() == 'r') { ticker.detach(); // write something event here ticker.attach_us(eventFunc, 1000000.0f/sampleFreq); } /* imu[i]->read_all(); printf("%10.3f,%10.3f,%10.3f,%10.3f,%10.3f,%10.3f,%10.3f,%10.3f,%10.3f,%10.3f ", imu[i]->Temperature, imu[i]->gyroscope_data[0], imu[i]->gyroscope_data[1], imu[i]->gyroscope_data[2], imu[i]->accelerometer_data[0], imu[i]->accelerometer_data[1], imu[i]->accelerometer_data[2], imu[i]->Magnetometer[0], imu[i]->Magnetometer[1], imu[i]->Magnetometer[2] );*/ //myled = 0; //wait(0.5); } }