imu_fusion
Dependencies: ICM20602_I2C IMU_fusion QMC5883L ledControl2 mbed
Fork of IMU_fusion by
Diff: main.cpp
- Revision:
- 3:788eecfd5ae9
- Parent:
- 2:3881894aaff5
--- a/main.cpp Thu Aug 06 12:25:49 2015 +0000 +++ b/main.cpp Wed Jul 19 07:59:55 2017 +0000 @@ -33,39 +33,91 @@ */ #include "mbed.h" -#include "HMC5883L.h" -#include "MPU6050.h" +#include "QMC5883L.h" +#include "icm20602_i2c.h" #include "ledControl.h" Serial pc(USBTX,USBRX); -MPU6050 mpu6050; -HMC5883L hmc5883l; +ICM20602 icm20602; +QMC5883L qmc5883l; Ticker toggler1; Ticker filter; Ticker compass; +enum detect_orientation_return +{ + DETECT_ORIENTATION_UPSIDE_DOWN, + DETECT_ORIENTATION_RIGHTSIDE_UP, + DETECT_ORIENTATION_LEFT, + DETECT_ORIENTATION_RIGHT, + DETECT_ORIENTATION_TAIL_DOWN, + DETECT_ORIENTATION_NOSE_DOWN, + DETECT_ORIENTATION_ERROR +}; + void toggle_led1(); void toggle_led2(); -void compFilter(); -void tiltCompensatedAngle(); +//void compFilter(); +void read_IMU_data(); +void read_MAG_data(); +void IMU_calibration(); +void IMU_compensate(); +char detect_orientation(float acc_dete[3], float gyro_dete[3]); +//void tiltCompensatedAngle(); float pitchAngle = 0; float rollAngle = 0; float yawAngle = 0; +float acc[3]; +float gyro[3]; +float acc_comp[3]; +float gyro_comp[3]; +float acc_off[3]; +float gyro_off[3]; +float mag[3]; +float IMU_tmp; +float mag_tmp; +const char cali = 'c'; +const char coll = ' '; +char orientation; int main() { + pc.baud(9600); // baud rate: 9600 - mpu6050.whoAmI(); // Communication test: WHO_AM_I register reading - mpu6050.calibrate(accelBias,gyroBias); // Calibrate MPU6050 and load biases into bias registers - mpu6050.init(); // Initialize the sensor - hmc5883l.init(); - filter.attach(&compFilter, 0.005); // Call the complementaryFilter func. every 5 ms (200 Hz sampling period) - compass.attach(&tiltCompensatedAngle, 0.015); // Call the tiltCompensatedAngle func. every 15 ms (75 Hz sampling period) + icm20602.whoAmI(); // Communication test: WHO_AM_I register reading + qmc5883l.ChipID(); +// icm20602.calibrate(accelBias,gyroBias); // Calibrate MPU6050 and load biases into bias registers +// filter.attach(&compFilter, 2); // Call the complementaryFilter func. every 5 ms (200 Hz sampling period) +// compass.attach(&tiltCompensatedAngle, 0.015); // Call the tiltCompensatedAngle func. every 15 ms (75 Hz sampling period) + + icm20602.init(); + qmc5883l.init(); while(1) - { - pc.printf("%.1f,%.1f,%.1f\r\n",rollAngle,pitchAngle,yawAngle); // send data to matlab - wait_ms(40); + { +// pc.putc(pc.getc()); +// pc.printf("%c\n",pc.getc()); + //if(pc.getc()==cali) +// { +// pc.printf("calibrate IMU!\n"); +// IMU_calibration(); +//// break; +// }else{ +// while(1) { + read_IMU_data(); + read_MAG_data(); + IMU_compensate(); + // pc.printf("%.5f,%.5f\r\n",aRes,gRes); // send data to matlab + pc.printf("original data:%.5f,%.5f,%.5f,%.5f,%.5f,%.5f,%.5f,%.5f,%.5f,%.5f,%.5f\r\n",acc[0],acc[1],acc[2],gyro[0],gyro[1],gyro[2],IMU_tmp,mag[0],mag[1],mag[2],mag_tmp); + // pc.printf("original acc:%.5f,%.5f,%.5f\r\n",acc[0],acc[1],acc[2]); // send data to matlab +// pc.printf("original gyro:%.5f,%.5f,%.5f\r\n",gyro[0],gyro[1],gyro[2]); // send data to matlab +// pc.printf("compensated acc:%.5f,%.5f,%.5f\r\n",acc_comp[0],acc_comp[1],acc_comp[2]); // send data to matlab +// pc.printf("compensated gyro:%.5f,%.5f,%.5f\r\n",gyro_comp[0],gyro_comp[1],gyro_comp[2]); // send data to matlab + // pc.printf("mag:%.5f,%.5f,%.5f\r\n",mag[0],mag[1],mag[2]); // send data to matlab + // wait_ms(400); +// ledToggle(3); + // } +// } } } @@ -73,40 +125,158 @@ void toggle_led2() {ledToggle(2);} /* This function is created to avoid address error that caused from Ticker.attach func */ -void compFilter() {mpu6050.complementaryFilter(&pitchAngle, &rollAngle);} - +//void compFilter() {icm20602.complementaryFilter(&pitchAngle, &rollAngle);} +//void read_imu() {icm20602.read_IMU_data(&acc[3], &gyro[3]);} /* Tilt compensated compass data */ // Works well for tilt in +/- 50 deg range -void tiltCompensatedAngle() -{ - float mag_Data[3], Xh, Yh; - hmc5883l.readMagData(mag_Data); - - Xh = mag_Data[0] * cos(rollAngle*PI/180) - mag_Data[2] * sin(rollAngle*PI/180) ; +//void tiltCompensatedAngle() +//{ +// float mag_Data[3], Xh, Yh; +// hmc5883l.readMagData(mag_Data); +// +// Xh = mag_Data[0] * cos(rollAngle*PI/180) - mag_Data[2] * sin(rollAngle*PI/180) ; +// +// Yh = mag_Data[0] * sin(pitchAngle*PI/180) * sin(rollAngle*PI/180) + +// mag_Data[1] * cos(pitchAngle*PI/180) - +// mag_Data[2] * sin(pitchAngle*PI/180) * cos(rollAngle*PI/180) ; +// +// /* Calculate the compensated heading angle */ +// double heading = atan2(Yh, Xh); +// +// // After calculating heading declination angle should be added to heading which is the error of the magnetic field in specific location. +// // declinationAngle can be found here http://www.magnetic-declination.com/ +// // For Ankara (my location) declinationAngle is ~5.5 degrees (0.096 radians) +// float declinationAngle = 0.096; +// heading += declinationAngle; +// +// // Correct for when signs are reversed. +// if(heading < 0) +// heading += 2*PI; +// +// // Check for wrap due to addition of declination. +// if(heading > 2*PI) +// heading -= 2*PI; +// +// /* Convert radian to degrees */ +// heading = heading * 180 / PI; +// +// yawAngle = heading; +//} +void read_IMU_data() { - Yh = mag_Data[0] * sin(pitchAngle*PI/180) * sin(rollAngle*PI/180) + - mag_Data[1] * cos(pitchAngle*PI/180) - - mag_Data[2] * sin(pitchAngle*PI/180) * cos(rollAngle*PI/180) ; + acc[0] = icm20602.getAccXvalue() * IMU_ONE_G * aRes; + acc[1] = icm20602.getAccYvalue() * IMU_ONE_G * aRes; + acc[2] = icm20602.getAccZvalue() * IMU_ONE_G * aRes; + gyro[0] = icm20602.getGyrXvalue() * gRes; + gyro[1] = icm20602.getGyrYvalue() * gRes; + gyro[2] = icm20602.getGyrZvalue() * gRes; + IMU_tmp = icm20602.getIMUTemp() / 262.144;//326.8; +// pc.printf("acc0:%.5f,%.5f,%.5f\r\n",acc[0],acc[1],acc[2]); // send data to matlab +} +void read_MAG_data() { - /* Calculate the compensated heading angle */ - double heading = atan2(Yh, Xh); + mag[0] = qmc5883l.getMagXvalue() * mRes; + mag[1] = qmc5883l.getMagYvalue() * mRes; + mag[2] = qmc5883l.getMagZvalue() * mRes; + mag_tmp = qmc5883l.getMagTemp() /262.144;// 100; +// pc.printf("mag:%.5f,%.5f,%.5f\r\n",mag[0],mag[1],mag[2]); // send data to matlab +} + +void IMU_calibration(){ + + int ii,jj,i=1,counter=0,timer=10; + float acc_cal[6][3]; + float gyro_cal[6][3]; + //float acc_sum[3]; +// float gyro_sum[3]; + pc.printf("put the IMU still!\n"); - // After calculating heading declination angle should be added to heading which is the error of the magnetic field in specific location. - // declinationAngle can be found here http://www.magnetic-declination.com/ - // For Ankara (my location) declinationAngle is ~5.5 degrees (0.096 radians) - float declinationAngle = 0.096; - heading += declinationAngle; - - // Correct for when signs are reversed. - if(heading < 0) - heading += 2*PI; + for(i=0;i<6;i++){ + + if(pc.getc()==coll){ + pc.printf("%dst side start\n",i+1); + float acc_sum[3]={0}; + float gyro_sum[3]={0}; + while(counter < timer){ + read_IMU_data(); + acc_sum[0] += acc[0]; + acc_sum[1] += acc[1]; + acc_sum[2] += acc[2]; + + gyro_sum[0] += gyro[0]; + gyro_sum[1] += gyro[1]; + gyro_sum[2] += gyro[2]; + + counter++; + //pc.printf(" acc :%.5f,%.5f,%.5f\r\n",acc[0],acc[1],acc[2]); // send data to matlab +// pc.printf(" gyro :%.5f,%.5f,%.5f\r\n",gyro[0],gyro[1],gyro[2]); // send data to matlab +// pc.printf(" acc sum:%.5f,%.5f,%.5f\r\n",acc_sum[0],acc_sum[1],acc_sum[2]); // send data to matlab +// pc.printf(" gyro sum:%.5f,%.5f,%.5f\r\n",gyro_sum[0],gyro_sum[1],gyro_sum[2]); // send data to matlab +// pc.printf("%d----\n",counter); + } +//pc.printf(" acc :%.5f,%.5f,%.5f\r\n",acc[0],acc[1],acc[2]); // send data to matlab +// pc.printf(" gyro :%.5f,%.5f,%.5f\r\n",gyro[0],gyro[1],gyro[2]); // send data to matlab + acc_cal[i][0] = acc_sum[0]/timer; + acc_cal[i][1] = acc_sum[1]/timer; + acc_cal[i][2] = acc_sum[2]/timer; + + gyro_cal[i][0] = gyro_sum[0]/timer; + gyro_cal[i][1] = gyro_sum[1]/timer; + gyro_cal[i][2] = gyro_sum[2]/timer; + pc.printf("%d--counter\n",counter); + counter = 0; + //acc_sum[0] =0.0;acc_sum[1] =0.0;acc_sum[2] =0.0; +// gyro_sum[0] = 0.0;gyro_sum[1] = 0.0;gyro_sum[2] = 0.0; +//pc.printf(" acc sum:%.5f,%.5f,%.5f\r\n",acc_cal[i][0],acc_cal[i][1],acc_cal[i][2]); // send data to matlab +// pc.printf(" gyro sum:%.5f,%.5f,%.5f\r\n",gyro_cal[i][0],gyro_cal[i][1],gyro_cal[i][2]); // send data to matlab + orientation = detect_orientation(acc_cal[i],gyro_cal[i]); + + pc.printf("%cst side completed\n",orientation); + } + } + //calculate the offset and scale + for(ii = 0;ii<3;ii++) + { + for(jj=0;jj<6;jj++){ + acc_off[ii] += acc_cal[jj][ii]; + gyro_off[ii] += gyro_cal[jj][ii]; + } + // pc.printf(" acc offset sum:%.5f,%.5f,%.5f\r\n",acc_off[0],acc_off[1],acc_off[2]); // send data to matlab +// pc.printf(" gyro offset sum:%.5f,%.5f,%.5f\r\n",gyro_off[0],gyro_off[1],gyro_off[2]); // send data to matlab + acc_off[ii]/=6; + gyro_off[ii]/=6; + } + //pc.printf(" acc offset:%.5f,%.5f,%.5f\r\n",acc_off[0],acc_off[1],acc_off[2]); // send data to matlab +// pc.printf(" gyro offset:%.5f,%.5f,%.5f\r\n",gyro_off[0],gyro_off[1],gyro_off[2]); // send data to matlab + return; +} + +char detect_orientation(float acc_dete[3], float gyro_dete[3]) +{ + if(fabsf(acc_dete[0]) < 1.0 && fabsf(acc_dete[1]) < 1.0 && fabsf(acc_dete[2] - IMU_ONE_G) < 1.0){ + return DETECT_ORIENTATION_UPSIDE_DOWN;//[0 0 g] + }else if(fabsf(acc_dete[0]) < 1.0 && fabsf(acc_dete[1]) < 1.0 && fabsf(acc_dete[2] + IMU_ONE_G) < 1.0){ + return DETECT_ORIENTATION_RIGHTSIDE_UP;//[0 0 -g] + }else if(fabsf(acc_dete[0]) < 1.0 && fabsf(acc_dete[1] - IMU_ONE_G) < 1.0 && fabsf(acc_dete[2]) < 1.0){ + return DETECT_ORIENTATION_LEFT;//[0 g 0] + }else if(fabsf(acc_dete[0]) < 1.0 && fabsf(acc_dete[1] + IMU_ONE_G) < 1.0 && fabsf(acc_dete[2]) < 1.0){ + return DETECT_ORIENTATION_RIGHT;//[0 -g 0] + }else if(fabsf(acc_dete[0] - IMU_ONE_G) < 1.0 && fabsf(acc_dete[1]) < 1.0 && fabsf(acc_dete[2]) < 1.0){ + return DETECT_ORIENTATION_TAIL_DOWN;//[g 0 0] + }else if(fabsf(acc_dete[0] + IMU_ONE_G) < 1.0 && fabsf(acc_dete[1]) < 1.0 && fabsf(acc_dete[2]) < 1.0){ + return DETECT_ORIENTATION_NOSE_DOWN;//[-g 0 0] + }else{ + return DETECT_ORIENTATION_ERROR; + } +} + +void IMU_compensate() +{ + int k; + for(k=0;k<3;k++) + { + acc_comp[k] = acc[k] - acc_off[k]; + gyro_comp[k] = gyro[k] - gyro_off[k]; + } - // Check for wrap due to addition of declination. - if(heading > 2*PI) - heading -= 2*PI; - - /* Convert radian to degrees */ - heading = heading * 180 / PI; - - yawAngle = heading; -} +} \ No newline at end of file