Diff: main.cpp

Revision:

2:b9549dd058d8

Parent:

1:1ad86845f584

--- a/main.cpp	Sat Jul 14 08:17:08 2018 +0000
+++ b/main.cpp	Sat Jul 14 10:10:50 2018 +0000
@@ -9,6 +9,7 @@
     Timer t;
     Serial pc(USBTX, USBRX); // tx, rx
     double acx,acy,acz;
+    int k = 0,l = 0;
     
 int main()
 {
@@ -68,7 +69,6 @@
     
     while(1) ; // Loop forever if communication doesn't happen
     }
-
     mpu9250.getAres(); // Get accelerometer sensitivity
     mpu9250.getGres(); // Get gyro sensitivity
     mpu9250.getMres(); // Get magnetometer sensitivity
@@ -185,16 +185,103 @@
     sum = 0;
     sumCount = 0; 
     
+    int flag = 0;
+    //落下判定のつもり
+    while(flag = (acz > 800 && acx < 150 && acx > -150 && acy < 300 && acy > -300)){
+        if(flag = 0)break;
+        pc.printf("*********************\n\r");
+            mpu9250.getAres(); // Get accelerometer sensitivity
+    mpu9250.getGres(); // Get gyro sensitivity
+    mpu9250.getMres(); // Get magnetometer sensitivity
+    pc.printf("Accelerometer sensitivity is %f LSB/g \n\r", 1.0f/aRes);
+    pc.printf("Gyroscope sensitivity is %f LSB/deg/s \n\r", 1.0f/gRes);
+    pc.printf("Magnetometer sensitivity is %f LSB/G \n\r", 1.0f/mRes);
+    magbias[0] = +470.;  // User environmental x-axis correction in milliGauss, should be automatically calculated
+    magbias[1] = +120.;  // User environmental x-axis correction in milliGauss
+    magbias[2] = +125.;  // User environmental x-axis correction in milliGauss
+
+ while(1) {
+  
+  // If intPin goes high, all data registers have new data
+  if(mpu9250.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) {  // On interrupt, check if data ready interrupt
+
+    mpu9250.readAccelData(accelCount);  // Read the x/y/z adc values   
+    // Now we'll calculate the accleration value into actual g's
+    ax = (float)accelCount[0]*aRes - accelBias[0];  // get actual g value, this depends on scale being set
+    ay = (float)accelCount[1]*aRes - accelBias[1];   
+    az = (float)accelCount[2]*aRes - accelBias[2];  
+   
+    mpu9250.readGyroData(gyroCount);  // Read the x/y/z adc values
+    // Calculate the gyro value into actual degrees per second
+    gx = (float)gyroCount[0]*gRes - gyroBias[0];  // get actual gyro value, this depends on scale being set
+    gy = (float)gyroCount[1]*gRes - gyroBias[1];  
+    gz = (float)gyroCount[2]*gRes - gyroBias[2];   
+  
+    mpu9250.readMagData(magCount);  // Read the x/y/z adc values   
+    // Calculate the magnetometer values in milliGauss
+    // Include factory calibration per data sheet and user environmental corrections
+    mx = (float)magCount[0]*mRes*magCalibration[0] - magbias[0];  // get actual magnetometer value, this depends on scale being set
+    my = (float)magCount[1]*mRes*magCalibration[1] - magbias[1];  
+    mz = (float)magCount[2]*mRes*magCalibration[2] - magbias[2];   
+  }
+   
+    Now = t.read_us();
+    deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
+    lastUpdate = Now;
     
+    sum += deltat;
+    sumCount++;
+    
+//    if(lastUpdate - firstUpdate > 10000000.0f) {
+//     beta = 0.04;  // decrease filter gain after stabilized
+//     zeta = 0.015; // increasey bias drift gain after stabilized
+ //   }
+    
+   // Pass gyro rate as rad/s
+//  mpu9250.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f,  my,  mx, mz);
+  mpu9250.MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
+    //出力されるのはここから
+    // Serial print and/or display at 0.5 s rate independent of data rates
+    delt_t = t.read_ms() - count;
+    if (delt_t > 500) { // update LCD once per half-second independent of read rate
+    
+    acx = 1000*ax;
+    acy = 1000*ay;
+    acz = 1000*az;
+    
+    break;
+/*    
+    pc.printf(" ax = %f", 1000*ax); 
+    pc.printf(" ay = %f", 1000*ay); 
+    pc.printf(" az = %f  mg\n\r", 1000*az); 
+
+    pc.printf(" gx = %f", gx); 
+    pc.printf(" gy = %f", gy); 
+    pc.printf(" gz = %f  deg/s\n\r", gz); 
+    
+    pc.printf(" mx = %f", mx); 
+    pc.printf(" my = %f", my); 
+    pc.printf(" mz = %f  mG\n\r", mz); 
+*/
+    }
+    
+/*  平均値とる方向性もなしで  
     double ac[3] = {0};
     do{
-        for(int j = 0;j < 3;j++){
-            for(int i = 0;i < 30;i++){
-                ac[j] += sqrt(pow(acx,2.0) + pow(acy,2.0) + pow(acz,2.0));
+        for(l;l < 3;l++){
+            for(k;k < 30;k += 0){
+                ac[l] += sqrt(pow(acx,2.0) + pow(acy,2.0) + pow(acz,2.0));
+                if(k < 28){
+                    k++;
+                    pc.printf("************%d巡目%d回目***********\n\r",l,k);
+                    goto Getdata;
+                }else k++;
             }
-            ac[j] /= 30;
-            pc.printf("%The Average is・・・%f\n\r",ac[0]);
+            k = 0;
+            ac[l] /= 30;
+            pc.printf("平均値は・・・%f\n\r",ac[l]);
         }
+        l = 0;
     }while(ac[1] > ac[0] && ac[1] < ac[2]);
     pc.printf("ループから抜けた\n\r");
 /*    while(1) {
@@ -212,3 +299,5 @@
 }
 } 
 }
+}
+}