Diff: main.cpp

Revision:

29:2da9b8d03c0b

Parent:

26:50272431cd1e

Child:

30:66fa18093418

--- a/main.cpp	Wed Jan 25 12:24:10 2017 +0000
+++ b/main.cpp	Fri Feb 17 07:38:36 2017 +0000
@@ -3,7 +3,8 @@
 #include "mbed.h"
 #include "rtos.h"
 #include "Fusokukei.h"
-#include "MPU6050.h"
+//#include "xMPU6050.h"
+#include "MPU6050_DMP6.h"
 #include "SDFileSystem.h"
 #include "BufferedSoftSerial.h"
 #include "Cadence.h"
@@ -56,7 +57,10 @@
 
 float sum = 0;
 uint32_t sumCount = 0;
-MPU6050 mpu6050;
+//MPU6050 mpu6050;
+float yaw,pitch,roll;
+float pre_ypr[3];
+float offset_ypr[3];
 Timer t;
 //
 //AnalogIn kx_X(p17);
@@ -73,7 +77,7 @@
 
 DigitalOut RollAlarmR(p20);
 DigitalOut RollAlarmL(p19);
-//DigitalOut led(LED1);
+//DigitalOut led(LED1);//for mpu
 DigitalOut led2(LED2);
 DigitalOut led3(LED3);
 DigitalOut led4(LED4);
@@ -117,6 +121,7 @@
 void call_calcAirSpeed(){
     air.calcAirSpeed(air_kaitensu);
     air_kaitensu = 0;
+    led3 = !led3;
 }
 
 //void sonarInterruptStart(){
@@ -147,7 +152,7 @@
 }
 
 void FusokukeiInit(){
-    FusokukeiPin.rise(air_countUp);
+    FusokukeiPin.rise(&air_countUp);
     FusokukeiTicker.attach(&call_calcAirSpeed, AIR_LOOP_TIME);
 }
 
@@ -163,27 +168,35 @@
     }
     fprintf(fp, "Hello fun SD Card World!\n\r");
     fclose(fp);
-//    Thread sd_thread(&SDprintf);
 }
+
 void MpuInit(){
-    i2c.frequency(400000);  // use fast (400 kHz) I2C
-    t.start();
-    uint8_t whoami = mpu6050.readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050);  // Read WHO_AM_I register for MPU-6050
-    if (whoami == 0x68) { // WHO_AM_I should always be 0x68
-        Thread::wait(1);
-        mpu6050.MPU6050SelfTest(SelfTest); // Start by performing self test and reporting values
-        Thread::wait(1);
-        if(SelfTest[0] < 1.0f && SelfTest[1] < 1.0f && SelfTest[2] < 1.0f && SelfTest[3] < 1.0f && SelfTest[4] < 1.0f && SelfTest[5] < 1.0f) {
-            mpu6050.resetMPU6050(); // Reset registers to default in preparation for device calibration
-            mpu6050.calibrateMPU6050(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
-            mpu6050.initMPU6050(); //pc.printf("MPU6050 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
-            Thread::wait(2);
-        } else {
-        }
-    } else {
-        pc.printf("MPU6050 not ready...\n\r");
-        //pc.printf("out\n\r"); // Loop forever if communication doesn't happen
-    }   
+//    i2c.frequency(400000);  // use fast (400 kHz) I2C
+//    t.start();
+//    uint8_t whoami = mpu6050.readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050);  // Read WHO_AM_I register for MPU-6050
+//    if (whoami == 0x68) { // WHO_AM_I should always be 0x68
+//        Thread::wait(1);
+//        mpu6050.MPU6050SelfTest(SelfTest); // Start by performing self test and reporting values
+//        Thread::wait(1);
+//        if(SelfTest[0] < 1.0f && SelfTest[1] < 1.0f && SelfTest[2] < 1.0f && SelfTest[3] < 1.0f && SelfTest[4] < 1.0f && SelfTest[5] < 1.0f) {
+//            mpu6050.resetMPU6050(); // Reset registers to default in preparation for device calibration
+//            mpu6050.calibrateMPU6050(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
+//            mpu6050.initMPU6050(); //pc.printf("MPU6050 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
+//            Thread::wait(2);
+//        } else {
+//        }
+//    } else {
+//        pc.printf("MPU6050 not ready...\n\r");
+//        //pc.printf("out\n\r"); // Loop forever if communication doesn't happen
+//    }
+    using namespace MPU6050DMP6;
+    setup();
+    do{
+        for(int i = 0; i<3; i++) pre_ypr[i] = ypr[i];
+        getYPR();
+        Thread::wait(0.01);
+    }while( (pre_ypr[1]-ypr[1]<0.0003)&&(pre_ypr[2]-ypr[2]<0.0003) );
+    for(int i = 0; i<3; i++) offset_ypr[i] = ypr[i];
 }
 
 //void SonarInit(){
@@ -197,53 +210,73 @@
         for(int i =0; i<20; i++){
             sonarV += sonarPin.read();
             Thread::wait(0.01);
+//            wait(0.01);
         }
         sonarDist = (sonarV/20)*2062.5;
         Thread::wait(0.01);
+//        wait(0.01);
     }
 }
 
+//void mpuProcessing(void const *argument){
+//    MpuInit();
+//    while(1){
+//        Thread::wait(0.1);
+//        if(mpu6050.readByte(MPU6050_ADDRESS, INT_STATUS) & 0x01) {  // check if data ready interrupt
+//            mpu6050.readAccelData(accelCount);  // Read the x/y/z adc values
+//            mpu6050.getAres();
+//            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];
+//            mpu6050.readGyroData(gyroCount);  // Read the x/y/z adc values
+//            mpu6050.getGres();
+//            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];
+//            tempCount = mpu6050.readTempData();  // Read the x/y/z adc values
+//            temperature = (tempCount) / 340. + 36.53; // Temperature in degrees Centigrade
+//        }
+//        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
+//        }
+//        mpu6050.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f);
+//        delt_t = t.read_ms() - count;
+//        if (delt_t > MPU_DELT_MIN) {
+//            yaw   = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);
+//            pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
+//            roll  = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
+//            pitch *= 180.0f / PI;
+//            yaw   *= 180.0f / PI;
+//            roll  *= 180.0f / PI;
+//            myled= !myled;
+//            count = t.read_ms();
+//            sum = 0;
+//            sumCount = 0;
+//        }
+//    }
+//}
 void mpuProcessing(void const *argument){
-    MpuInit();
+    using namespace MPU6050DMP6;
+    setup();
+    do{
+        for(int i = 0; i<3; i++) pre_ypr[i] = ypr[i];
+        getYPR();
+        Thread::wait(0.01);
+    }while( (pre_ypr[1]-ypr[1]<0.0003)&&(pre_ypr[2]-ypr[2]<0.0003) );
+    for(int i = 0; i<3; i++) offset_ypr[i] = ypr[i];
+//    MpuInit();
     while(1){
-        Thread::wait(0.1);
-        if(mpu6050.readByte(MPU6050_ADDRESS, INT_STATUS) & 0x01) {  // check if data ready interrupt
-            mpu6050.readAccelData(accelCount);  // Read the x/y/z adc values
-            mpu6050.getAres();
-            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];
-            mpu6050.readGyroData(gyroCount);  // Read the x/y/z adc values
-            mpu6050.getGres();
-            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];
-            tempCount = mpu6050.readTempData();  // Read the x/y/z adc values
-            temperature = (tempCount) / 340. + 36.53; // Temperature in degrees Centigrade
-        }
-        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
-        }
-        mpu6050.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f);
-        delt_t = t.read_ms() - count;
-        if (delt_t > MPU_DELT_MIN) {
-            yaw   = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);
-            pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
-            roll  = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
-            pitch *= 180.0f / PI;
-            yaw   *= 180.0f / PI;
-            roll  *= 180.0f / PI;
-            myled= !myled;
-            count = t.read_ms();
-            sum = 0;
-            sumCount = 0;
-        }
+        getYPR();
+        yaw = (ypr[0] - offset_ypr[0]) *180/M_PI;
+        pitch = (ypr[1] - offset_ypr[1]) *180/M_PI;
+        roll = (ypr[2] - offset_ypr[2]) *180/M_PI;
+        Thread::wait(0.01);
     }
 }
 
@@ -297,12 +330,12 @@
         //writeDatas[write_datas_index][i] = 0.0;    
         writeDatas[write_datas_index][i] = (float)soudaDatas[i+6];
     }
-    writeDatas[write_datas_index][i++] = cadence.cadence;
-    writeDatas[write_datas_index][i++] = sonarDist;
     writeDatas[write_datas_index][i++] = pitch;
     writeDatas[write_datas_index][i++] = roll;
     writeDatas[write_datas_index][i++] = yaw;
     writeDatas[write_datas_index][i++] = airSpeed;
+    writeDatas[write_datas_index][i++] = sonarDist;
+    writeDatas[write_datas_index][i++] = cadence.cadence;
     //writeDatas[write_datas_index][i++] = writeTimer.read();
     for(i = 0; i < WRITE_DATAS_NUM; i++){
         pc.printf("%f   ", writeDatas[write_datas_index][i]);
@@ -319,13 +352,11 @@
         twe.printf("%i,",soudaDatas[i]);
         ssMutex.unlock();
     }
-//    twe.printf("%f\n\r",cadence.cadence);
-//    pc.printf("%f\n\r",cadence.cadence);
-    //pc.printf("\n\r");
-
-if(Android.writeable()){
-    Android.printf("%f,%f,%f",airSpeed,roll,0);
-}
+    
+    if(Android.writeable()){
+        Android.printf("%f,%f,%f",airSpeed,roll,0);
+    }
+    
     ssMutex.lock();
     twe.printf("%f,%f,%f,",pitch,roll,yaw);
 //    twe.printf("%f,%f,%f,",calcKXdeg(kx_X.read()),calcKXdeg(KX_Y),calcKXdeg(KX_Z));   
@@ -341,7 +372,7 @@
 void WriteDatasF(){
     pc.printf("airSpeed:%f\n\r",airSpeed);
 }
-//
+
 //float calcKXdeg(float x){
 //    return -310.54*x+156.65;
 //}
@@ -367,7 +398,7 @@
 
 
 
-int main(){
+int main(){ 
     Thread sd_thread(&SDprintf);//必ずmain内で
     Thread sonar_thread(&sonarCalc);
     Thread mpu_thread(&mpuProcessing);
@@ -376,10 +407,10 @@
     init();
     while(1){
         pc.printf("test\n\r");
-//        mpuProcessing();
         RollAlarm();
 //        DataReceiveFromSouda();
 //        cadence.readData();
         WriteDatas();
+//        mpuProcessing();
     }
 }
\ No newline at end of file