Diff: main.cpp

Revision:

32:fe006d07c1ea

Parent:

30:66fa18093418

--- a/main.cpp	Fri Feb 17 09:06:58 2017 +0000
+++ b/main.cpp	Fri Feb 17 12:14:23 2017 +0000
@@ -1,6 +1,7 @@
 //計器プログラム
 
 #include "mbed.h"
+#define __mbed_h__
 #include "rtos.h"
 #include "Fusokukei.h"
 //#include "xMPU6050.h"
@@ -83,8 +84,8 @@
 SDFileSystem sd(p5, p6, p7, p8, "sd");
 FILE* fp;
 
-PwmOut kisokuServo(p26);
-PwmOut geikakuServo(p22);
+//PwmOut kisokuServo(p26);
+//PwmOut geikakuServo(p22);
 
 char soudaDatas[SOUDA_DATAS_NUM];
 float writeDatas[SD_WRITE_NUM][WRITE_DATAS_NUM];
@@ -103,6 +104,7 @@
 void DataReceiveFromSouda(void const *argument);
 void SDprintf(void const *argument);
 void WriteDatas();
+void calcPusle(int deg);
 //float calcAttackAngle();
 //float calcKXdeg(float x);
 
@@ -142,8 +144,8 @@
     twe.baud(115200);
     Android.baud(9600);
     soudaSerial.baud(9600);
-    kisokuServo.period_ms(INIT_SERVO_PERIOD_MS);
-    geikakuServo.period_ms(INIT_SERVO_PERIOD_MS);
+//    kisokuServo.period_ms(INIT_SERVO_PERIOD_MS);
+//    geikakuServo.period_ms(INIT_SERVO_PERIOD_MS);
     FusokukeiInit();
 //    MpuInit(); //mpuProcessing内で
 //    SonarInit();
@@ -155,10 +157,6 @@
     FusokukeiTicker.attach(&call_calcAirSpeed, AIR_LOOP_TIME);
 }
 
-double calcPulse(int deg){
-    return (0.0006+(deg/180.0)*(0.00235-0.00045));
-}
-
 void SdInit(){
     mkdir("/sd/mydir", 0777);
     fp = fopen("/sd/mydir/sdtest2.csv", "w");
@@ -248,12 +246,15 @@
 //}
 void mpuProcessing(void const *argument){
     using namespace MPU6050DMP6;
+    led1 = 1;
     setup();
-//    do{
-//        for(int i = 0; i<3; i++) pre_ypr[i] = ypr[i];
+    int j=0;
+    do{
+        for(int i = 0; i<3; i++) pre_ypr[i] = ypr[i];
         getYPR();
-//        Thread::wait(0.05);
-//    }while( (pre_ypr[1]-ypr[1]<0.0003)&&(pre_ypr[2]-ypr[2]<0.0003) );
+        Thread::wait(0.05);
+        j++;
+    }while( (pre_ypr[1]-ypr[1]<0.0003)&&(pre_ypr[2]-ypr[2]<0.0003)&&j<20 );
     for(int i = 0; i<3; i++) offset_ypr[i] = ypr[i];
 //    MpuInit();
     while(1){
@@ -348,20 +349,20 @@
     } 
     for(int i = 0; i < SOUDA_DATAS_NUM; i++){
 //        pc.printf("%i   ",soudaDatas[i]);
-        ssMutex.lock();
+//        ssMutex.lock();
         twe.printf("%i,",soudaDatas[i]);
-        ssMutex.unlock();
+//        ssMutex.unlock();
     }
     
     if(Android.writeable()){
         Android.printf("%f,%f,%f",airSpeed,roll,0);
     }
     
-    ssMutex.lock();
+//    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));   
     twe.printf("%f,\r\n",airSpeed); 
-    ssMutex.unlock();
+//    ssMutex.unlock();
 //    pc.printf("%f,%f,%f\n\r",pitch,roll,yaw);
     //pc.printf("%f,%f,%f\n\r",calcKXdeg(kx_X.read()),calcKXdeg(KX_Y),calcKXdeg(KX_Z));   
 //    pc.printf("%f\n\r",airSpeed);
@@ -369,6 +370,10 @@
     pc.printf("%d\n\r",write_datas_index);
 }
 
+double calcPulse(int deg){
+    return (0.0006+(deg/180.0)*(0.00235-0.00045));
+}
+
 void WriteDatasF(){
     pc.printf("airSpeed:%f\n\r",airSpeed);
 }