Diff: main.cpp

Branch:

Thread-gyogetsuMPU

Revision:

33:69ad9920f693

Parent:

27:d2955f29a3aa

Child:

34:c46f2f687c7b

--- a/main.cpp	Sat Jan 28 10:17:50 2017 +0000
+++ b/main.cpp	Sat Feb 18 03:03:25 2017 +0000
@@ -1,6 +1,6 @@
 //計器プログラム
 #include "mbed.h"
-//#include "rtos.h"
+#include "rtos.h"
 #include "Fusokukei.h"
 #include "MPU6050.h"
 #include "BufferedSoftSerial.h"
@@ -109,7 +109,10 @@
 }
 
 void updateCadence(){
+    while(1){
         cadence_twe.readData();
+        Thread::wait(0.1);
+    }
 }
 
 void init(){
@@ -120,7 +123,7 @@
     twe.baud(19200);//BufferedSoftSerialでは19200が上限。twelite側でもBPS無効化が必要
     //writeTimer.start();
     FusokukeiInit();
-    MpuInit(); 
+//    MpuInit(); 
     //writeDatasTicker.attach(&WriteDatas,1);
 //    cadenceUpdateTicker.attach(&updateCadence, 1);
     
@@ -140,14 +143,14 @@
     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
-        wait(1);
+        Thread::wait(1);
         mpu6050.MPU6050SelfTest(SelfTest); // Start by performing self test and reporting values
-        wait(1);
+        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
-            wait(2);
+            Thread::wait(2);
         } else {
         }
     } else {
@@ -161,43 +164,47 @@
 }
 
 void mpuProcessing(){
-    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;
-    }
+    MpuInit();
+    while(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;
+        }
+        Thread::wait(0.01);
+    }//while(1)
 }
 
 void DataReceiveFromSouda(){
@@ -304,14 +311,15 @@
 
 int main(){
 //    Thread cadence_thread(&updateCadence);
+    Thread mpu_thread(&mpuProcessing);
     init();
     while(1){
         pc.printf("test\n\r");
-        mpuProcessing();
+//        mpuProcessing();
         sonarCalc();
         RollAlarm();
         DataReceiveFromSouda();
-        updateCadence();
+//        updateCadence();
         WriteDatas();
 //        WriteServo();
     }