Hexcopter distance measurement using IMU unit
Dependencies: mbed MPU6050_acc_CF ledControl
main.cpp
- Committer:
- mbedproject
- Date:
- 2016-06-15
- Revision:
- 1:54b66b7ca11e
- Parent:
- 0:ecc07e53ba65
File content as of revision 1:54b66b7ca11e:
// Program by Suraj Vatsya // Special thanks to @author: Baser Kandehir whose library is used to make this program // and to the original MPU6050 library creater Kris Winer's. // This program measure distance with the help of IMU // Only when motion is in one dimension i.e. along X axis #include "mbed.h" #include "MPU6050.h" #include "ledControl.h" // initialize serial Communication Serial pc(USBTX,USBRX); // default baud rate: 9600 MPU6050 mpu6050; // class: MPU6050, object: mpu6050 Ticker toggler1; Ticker filter; void compFilter(); //Timer defined Timer t; int s, p, m, q = 0; int i, r, u =0; int flag = 0; float sTotal = 0; float initAcc = 0; float sAvg[3] = {0}; float sData[1000] = {0}; float dData[1000] = {0}; float mData[1000] = {0}; float tData[1000] = {0}; float pos[1000] = {0}; float dD[1000] = {0}; float mTotal = 0; float sumAcc = 0; float avgAcc = 0; float motion = 0; float ac = 0; float realAcc =0; float vel = 0; float timeTaken =0; double dis = 0; double dis_m = 0; float tim=0; float ti=0; float pitchAngle = 0; float rollAngle = 0; unsigned int first=0, second = 0; int last, last_2, last_3, last_4 = 0; unsigned char bit_i=0; // Reset all variables void resetall() { m=0; flag = 0; i=0; sTotal = 0; // initAcc = 0; r=0; u=0; sumAcc =0; avgAcc = 0; ac = 0; dis =0; dis_m = 0; ti= 0; tim =0; q=0; } // To reset samples void resetsample() { sAvg[0] = 0; sAvg[1] = 0; } // To change Negative value void changval() { if(axx<0) { ac = (0 - axx); } else ac = axx; } // To collect Samples void callSample(int value) { // for(int v=0;v<2;v++) { // resetsample(); for(s=0;s<10;s++) { changval(); sData[s] = ac; sTotal += ac; // wait_ms(1); } sAvg[value] = (sTotal)/(10); sTotal = 0; } } // To check if there is any motion in the sensor/platform void checkmotion() { q=0; motion = sAvg[2] - initAcc; if(motion>1) { if(i==0){t.start(); ledControl(1,1);} m=1; {printf("Motion detected*\r");} } } // To check if platoform stopped moving // If not; do nothing // If yes; change flag bit m to zero void checkstop() { if((dData[i-1]) < 0.48 && (dData[i-2]) < 0.1 ) { m=0; } } // To accumulate Accelerometer & Time Readings void grabAcc() { for(i=0; m==1; i++) { for(r=0; r<5; r++) { changval(); mData[r] = ac; mTotal += ac; wait_ms(5); } dData[i] = ((mTotal)/5)-initAcc; tData[i] = t.read(); ledToggle(2); mTotal = 0; if(i>2) { checkstop(); } } ledControl(3,1); ledControl(2,1); ledControl(1,0); t.stop(); flag=1; } void calDistance() { for(u=0;u<(i-1);u++) { sumAcc += dData[u]; } avgAcc = sumAcc / (i-1); timeTaken = tData[i-1] - tData[0]; realAcc = avgAcc; dis = 0.5 * realAcc * (timeTaken * timeTaken); dis_m = 39.37 * dis; flag = 1; } // Initial acceleration value when sensor/platform is stationary and at leveled state void initAccel() { ledControl(1,1); ledControl(2,1); ledControl(3,1); ledControl(4,1); printf("Raise IMU sensor board and wait till initial readings is taken\r\n"); wait_ms(500); printf("Grabbing Initial Accelerometer reading in 3\r"); ledControl(4,0); wait_ms(500); printf("Grabbing Initial Accelerometer reading in 2\r"); ledControl(3,0); wait_ms(500); printf("Grabbing Initial Accelerometer reading in 3\r"); ledControl(2,0); wait_ms(400); printf("Process started"); callSample(0); wait_ms(5); callSample(1); wait_ms(5); initAcc = (sAvg[1] + sAvg[0])/2; printf("Initial Reading Process Completed\r\n"); printf("Motion process Started, Initial Acc = %f \r\n", initAcc); // printf("Motion process Started, 0 = %f 1= %f \r\n", sAvg[1], sAvg[0]); ledControl(1,1); ledControl(2,1); ledControl(3,1); ledControl(4,1); wait_ms(200); } // main program int main() { pc.baud(9600); wait_ms(100); // baud rate: 9600 mpu6050.checkaddress(); // Communication test: WHO_AM_I register reading wait(1); mpu6050.calibrate(accelBias,gyroBias); // Calibrate MPU6050 and load biases into bias registers pc.printf("Calibration Done. \r\n"); wait(0.5); mpu6050.init(); // Initialize the sensor wait(1); pc.printf("IMU is initialized for operation.. \r\n\r\n"); wait_ms(500); filter.attach(&compFilter, 0.005); // Call the complementaryFilter func. every 5 ms (200 Hz sampling period) // axx=ax*9.8f; ayy=ay*9.8f; azz=az*9.8f; initAccel(); //printf(" Acceleration (in m/s^2) Time (in seconds) \r\n"); while(1) { filter.attach(&compFilter, 0.005); // Call the complementaryFilter func. every 5 ms (200 Hz sampling period) // reset(); //------------------------Start Collecting Samples of Accelerometer Data--------------------------- callSample(2); wait_ms(5); callSample(3); wait_ms(5); // printf("Sample 0 = %f Sample 1 = %f Sample 2 = %f \r", sAvg[0], sAvg[1], sAvg[2]); checkmotion(); if(m==1) {grabAcc();} if(flag == 1) { calDistance(); printf("distance(in Meter) = %2.4f distance(in Inches) = %f \r\n", dis, dis_m); // wait(5); flag =0; } resetall(); t.reset(); } } /* This function is created to avoid address error that caused from Ticker.attach func */ void compFilter() {mpu6050.complementaryFilter(&pitchAngle, &rollAngle);}