imu_serial initial commit
Dependencies: MadgwickAHRS mpu9250 mbed
Diff: main.cpp
- Revision:
- 4:7fa9cfebb00c
- Parent:
- 3:c1902ecb30a7
--- a/main.cpp Thu Oct 06 17:35:14 2016 +0000 +++ b/main.cpp Thu Oct 06 23:42:28 2016 +0000 @@ -8,7 +8,7 @@ (1) hardcoded output - allow for an output object to change what interface data comes from (2) hardcoded input -allow for an input object to change what interface data goes to (3) robustness, sometimes needs to be rebooted. maybe the serial interface times out? -(4) optimization - lots of ineffeciencies in code, +(4) optimization - lots of ineffeciencies in code, (5) need to rerun some speed tests - find out how long it takes to get data from IMU, current sampling rate, if rate can be increased (6) have ?4? i2c ports, can maybe use all 4 to log faster? (7) need to run some speed tests on mux, find out how long it takes to switch a channel @@ -163,7 +163,7 @@ mpu9250[7].magbias[1] = 35.75; mpu9250[7].magbias[2] = -94.875; */ - + /* mpu9250[0].magbias[0] = 278.785; mpu9250[0].magbias[1] = 205.86; mpu9250[0].magbias[2] = 60.225; @@ -188,6 +188,14 @@ mpu9250[7].magbias[0] = -92.965; mpu9250[7].magbias[1] = 34.195; mpu9250[7].magbias[2] = -18.675; + */ + mpu9250[0].magbias[0] = 153.895; + mpu9250[0].magbias[1] = 347.325; + mpu9250[0].magbias[2] = -227.89; + mpu9250[1].magbias[0] = 158.285; + mpu9250[1].magbias[1] = 314.005; + mpu9250[1].magbias[2] = -302.905; + menu(pc); } // end of main @@ -428,11 +436,12 @@ mpu9250[i].Now = t.read_us(); mpu9250[i].deltat = (float)((mpu9250[i].Now - mpu9250[i].lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update mpu9250[i].lastUpdate = mpu9250[i].Now; - MadgwickAHRSupdate(mpu9250[i].gx*PI/180.0f, mpu9250[i].gy*PI/180.0f, mpu9250[i].gz*PI/180.0f, mpu9250[i].ax, mpu9250[i].ay, mpu9250[i].az, mpu9250[i].my, mpu9250[i].mx, -1.0f*mpu9250[i].mz); - mpu9250[i].q[0] = q0; - mpu9250[i].q[1] = q1; - mpu9250[i].q[2] = q2; - mpu9250[i].q[3] = q3; + //mpu9250[i].MadgwickAHRSupdate(mpu9250[i].gx*PI/180.0f, mpu9250[i].gy*PI/180.0f, mpu9250[i].gz*PI/180.0f, mpu9250[i].ax, mpu9250[i].ay, mpu9250[i].az, mpu9250[i].my, mpu9250[i].mx, -1.0f*mpu9250[i].mz, mpu9250[i].q[0], mpu9250[i].q[1], mpu9250[i].q[2], mpu9250[i].q[3]); + mpu9250[i].Madgwickupdate(); + //mpu9250[i].q[0] = q0; + //mpu9250[i].q[1] = q1; + //mpu9250[i].q[2] = q2; + //mpu9250[i].q[3] = q3; delt_t = t.read_ms() - count;