![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
9軸センサー動作確認
Dependencies: MPU9250_SPI mbed
Diff: main.cpp
- Revision:
- 0:645f2b28239f
diff -r 000000000000 -r 645f2b28239f main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Fri Mar 17 08:42:47 2017 +0000 @@ -0,0 +1,83 @@ +/*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); + } +} +