9軸センサー動作確認
Dependencies: MPU9250_SPI mbed
main.cpp
- Committer:
- tomoya123
- Date:
- 2017-03-17
- Revision:
- 0:645f2b28239f
File content as of revision 0:645f2b28239f:
/*CODED by Qiyong Mu on 21/06/2014 kylongmu@msn.com */ #include "mbed.h" #include "MPU9250.h" //Include library Serial pc(USBTX, USBRX); Serial xbee(p9,p10); DigitalOut myled(LED1); //Serial pc(USBTX, USBRX); SPI spi(p5, p6, p7); mpu9250_spi imu(spi,p8); //define the mpu9250 object int main(){ //float mx,my,mz,F; //float ax,ay,az,acc_roll,acc_pitch,calib_az,roll_deg; //float mag_x,mag_y,Orient; //int i; pc.baud(115200); //xbee.baud(115200); if(imu.init(1,BITS_DLPF_CFG_188HZ)){ //INIT the mpu9250 printf("\nCouldn't initialize MPU9250 via SPI!"); } printf("\nWHOAMI=0x%2x\n",imu.whoami()); //output the I2C address to know if SPI is working, it should be 104 wait(1); printf("Gyro_scale=%u\n",imu.set_gyro_scale(BITS_FS_2000DPS)); //Set full scale range for gyros wait(1); printf("Acc_scale=%u\n",imu.set_acc_scale(BITS_FS_16G)); //Set full scale range for accs wait(1); printf("AK8963 WHIAM=0x%2x\n",imu.AK8963_whoami()); wait(0.1); imu.AK8963_calib_Magnetometer(); imu.calib_acc(); pc.printf("good luck"); while(1) { wait(0.1); //imu.read_temp(); //imu.read_acc(); //imu.read_rot(); //imu.AK8963_read_Magnetometer(); imu.read_all(); xbee.printf("%f,%f,%f\r\n", //imu.accelerometer_data[0], //imu.accelerometer_data[1], //imu.accelerometer_data[2], imu.Magnetometer[0], imu.Magnetometer[1], imu.Magnetometer[2] ); /* ax=ay=az=0; for(i=0;i<100;i++){ ax = ax + imu.accelerometer_data[0]; ay = ay + imu.accelerometer_data[1]; az = az + imu.accelerometer_data[2]; } wait(1.0); ax=ax/100; ay=ay/100; az=az/100; acc_roll = atan2(ay,az); //roll_deg = acc_roll*180/3.1415; calib_az = ay*sin(acc_roll)+az*cos(acc_roll); acc_pitch = atan2(-ax,calib_az); mag_x = (imu.Magnetometer[2]+13.755)*sin(acc_roll)-(imu.Magnetometer[1] - 20.469)*cos(acc_roll); mag_y = (imu.Magnetometer[0] - 12.983)*cos(acc_pitch) +(imu.Magnetometer[1] - 20.469)*sin(acc_pitch)*sin(acc_roll)+(imu.Magnetometer[2]+13.755)*sin(acc_pitch)*cos(acc_roll); Orient = atan2(mag_x,mag_y)*180/3.1415; printf("Orientation=%10.5f\n",Orient);*/ //printf("roll=%10.5f,pitch=%10.5f\n",roll_deg,acc_pitch); // mx = (imu.Magnetometer[0]- 12.983)*cos(7*3.1415/180); //my = (imu.Magnetometer[1]- 20.469)*cos(7*3.1415/180); //mz = imu.Magnetometer[2]; // F = atan2(my,mx)*180/3.1415; //printf("degree=%10.5f\n",F); //myled = 0;*/ wait(0.5); } }