Diff: main.cpp

Branch:

cadence

Revision:

66:1ffafa1c4a87

Parent:

64:2de7ea84d39a

Child:

67:9d2eb6793464

--- a/main.cpp	Wed Jun 14 11:29:52 2017 +0000
+++ b/main.cpp	Thu Jun 15 08:30:38 2017 +0000
@@ -11,7 +11,7 @@
 #define SOUDA_DATAS_NUM 28 //(yokutan 7 + input 7)*2
 #define YOKUTAN_DATAS_NUM 14
 #define WRITE_DATAS_NUM 34 // souda_datas_num + 6( rpy, airspeed, height, cadence)
-#define SD_WRITE_NUM 20
+#define SD_WRITE_NUM 40
 #define MPU_LOOP_TIME 0.01
 #define AIR_LOOP_TIME 0.01//(0.002005)
 #define WRITE_DATAS_LOOP_TIME 1
@@ -77,7 +77,8 @@
 I2C InaI2c(p9,p10);
 INA226 VCmonitor(InaI2c,0x9C);
 AnalogIn mgPin(p20);
-AnalogIn mgPin2(p16);
+//AnalogIn mgPin2(p16);
+InterruptIn cadenceInter(p16);
 
 char soudaDatas[SOUDA_DATAS_NUM];
 float writeDatas[SD_WRITE_NUM][WRITE_DATAS_NUM];
@@ -87,7 +88,7 @@
 void call_calcAirSpeed();
 void sonarInterruptStart();
 void sonarInterruptStop();
-void updateCadence(double source, double input,double input2,bool isFFlag);
+void updateCadence(double source, double input,double input2);
 void init();
 void FusokukeiInit();
 void MpuInit();
@@ -160,23 +161,22 @@
 
 //ケイデンスの値を取得します。
 // source: 定格12V電源の電圧値[mV], input: センサ値[mV]
-void updateCadence(double source, double input,double input2,bool isFFlag)
+void updateCadence()
 {
+    static bool isFFlag = true;
     if(isFFlag) {
-        lastCadenceInput =  isOh182eOverThreshold(source,input);
-        lastCadenceInput2 =  isOh182eOverThreshold(source,input2);
         cadenceTimer.start();
+        isFFlag =  false;
         return;
     }
-    if((isOh182eOverThreshold(source,input) ^ lastCadenceInput) ||(isOh182eOverThreshold(source,input2) ^ lastCadenceInput2)) {
-        if(cadenceCounter < 4) {
-            cadenceCounter++;
-            return;
-        }
-        cadenceResult =60.0/  (cadenceTimer.read_us() / 1000000.0); //クランク一回転にかかる時間を取得
-        cadenceTimer.reset();
-        cadenceCounter = 0;
+    led4 = !led4;
+    if(cadenceCounter < 3) {
+        cadenceCounter++;
+        return;
     }
+    cadenceResult =60.0/  (cadenceTimer.read_us() / 1000000.0); //クランク一回転にかかる時間を取得
+    cadenceTimer.reset();
+    cadenceCounter = 0;
 }
 
 void init()
@@ -202,7 +202,6 @@
     val = 0;
     if(VCmonitor.rawRead(0x00,&val) != 0) {
         printf("VCmonitor READ ERROR\n");
-        while(1) {}
     }
     VCmonitor.setCurrentCalibration();
 }
@@ -303,11 +302,10 @@
 void SdInit()
 {
     mkdir("/sd/mydir", 0777);
-    fp = fopen("/sd/mydir/sdtest2.csv", "w");
+    fp = fopen("/sd/mydir/TestFlight.csv", "w");
     if(fp == NULL) {
         error("Could not open file for write\n");
     }
-    fprintf(fp, "Hello fun SD Card World!\n\r%f",0.1f);
     fclose(fp);
 }
 
@@ -360,22 +358,21 @@
     } else {
         write_datas_index++;
     }
-    char sbuf[128];
-    int p=0;
-//    twe.printf("con,");
-    p = sprintf(sbuf,"con,");
+//    char sbuf[128];
+//    int p=0;
+    twe.printf("con,");
+//    p = sprintf(sbuf,"con,");
     for(int i = 0; i <YOKUTAN_DATAS_NUM ; i++) {
-//        pc.printf("%i   ",soudaDatas[i]);
-//        twe.printf("%i,",soudaDatas[i]);
-        p = sprintf(sbuf+p,"%i,",soudaDatas[i]);
-
+////        pc.printf("%i   ",soudaDatas[i]);
+        twe.printf("%i,",soudaDatas[i]);
+//        p = sprintf(sbuf+p,"%i,",soudaDatas[i]);
+//
         if(i == YOKUTAN_DATAS_NUM - 1)
-//            twe.printf("%i\n",soudaDatas[i]);
-            p = sprintf(sbuf+p,"%i\n",soudaDatas[i]);
+            twe.printf("%i\n",soudaDatas[i]);
+//            p = sprintf(sbuf+p,"%i\n",soudaDatas[i]);
     }
-    twe.printf("%s",sbuf);
-    twe.printf("inp,%d,%i,%d,%i\n",soudaDatas[YOKUTAN_DATAS_NUM],soudaDatas[sizeof(int) + YOKUTAN_DATAS_NUM + 2],(int)soudaDatas[SOUDA_DATAS_NUM - sizeof(int) - 3],soudaDatas[SOUDA_DATAS_NUM-1]);
-    
+//    twe.printf("%s",sbuf);
+    //twe.printf("con,%s\n",soudaDatas);
     /*
     送信文字列
     0-13翼端データ
@@ -384,25 +381,13 @@
     19-22 L erebon
     23 LDRUG
     */
-    ////pc.printf("\n\r");
-    twe.printf("mpu,%f,%f,%f\n",pitch,roll,yaw);
-    twe.printf("kei,%f,%f,%f\n",airSpeed,sonarDist,cadenceResult);//cadence_twe.cadence);
-
-    ////pc.printf("%f,%f,%f\n\r",calcKXdeg(kx_X.read()),calcKXdeg(KX_Y),calcKXdeg(KX_Z));
-//    pc.printf("%f,%f,%f\n\r",airSpeed,sonarDist,cadenceResult);//cadence_twe.cadence);
-//    pc.printf("%d,%i,%d,%i,",soudaDatas[YOKUTAN_DATAS_NUM],soudaDatas[sizeof(int) + YOKUTAN_DATAS_NUM + 2],(int)soudaDatas[SOUDA_DATAS_NUM - sizeof(int) - 3],soudaDatas[SOUDA_DATAS_NUM-1]);
-//    pc.printf("%f,%f,%f\n\r",pitch,roll,yaw);
-//    printf("mgPin V:%f\n\r",mgPin.read()*3.3);
-    pc.printf("mgPin V:%f\n%d,%i,%d,%i\n%f,%f,%f\n%f,%f,%f\n\r",mgPin.read()*3.3,soudaDatas[YOKUTAN_DATAS_NUM],soudaDatas[sizeof(int) + YOKUTAN_DATAS_NUM + 2],(int)soudaDatas[SOUDA_DATAS_NUM - sizeof(int) - 3],soudaDatas[SOUDA_DATAS_NUM-1],pitch,roll,yaw,airSpeed,sonarDist,cadenceResult);
-//    for(int i = 0; i < strlen(cadence_twe.myBuff); i++){
-//        ////pc.printf("%c",*(cadence_twe.myBuff+i));
-//    }
-//    pc.printf("%f\t%f\t%f\t%f\n\r",airSpeed,air_sum[0],air_sum[1],air_sum[2]);
+    twe.printf("inp,%d,%i,%d,%i\nmpu,%f,%f,%f\nkei,%f,%f,%f\n",soudaDatas[YOKUTAN_DATAS_NUM],soudaDatas[sizeof(int) + YOKUTAN_DATAS_NUM + 2]
+               ,(int)soudaDatas[SOUDA_DATAS_NUM - sizeof(int) - 3],soudaDatas[SOUDA_DATAS_NUM-1]
+               ,pitch,roll,yaw,airSpeed,sonarDist,cadenceResult);//cadence_twe.cadence);
+    pc.printf("cadence:%5.2f\n\r",cadenceResult);
+    // pc.printf("%d,%i,%d,%i\n%f,%f,%f\n%f,%f,%f\n\r",soudaDatas[YOKUTAN_DATAS_NUM],soudaDatas[sizeof(int) + YOKUTAN_DATAS_NUM + 2],(int)soudaDatas[SOUDA_DATAS_NUM - sizeof(int) - 3],soudaDatas[SOUDA_DATAS_NUM-1],pitch,roll,yaw,airSpeed,sonarDist,cadenceResult);
     if(android.writeable()) {
-//        android.printf("%f,%f,%f,",pitch,roll,yaw);
-//        android.printf("%f,%f,\r\n",airSpeed,sonarDist);
         android.printf("%4.2f,%4.2f,%4.2f,\n,",roll,airSpeed,cadenceResult);//cadence_twe.cadence);
-//        led2 = !led2; 
     }
 //    SDprintf();
 }
@@ -412,10 +397,6 @@
     //pc.printf("airSpeed:%f\n\r",airSpeed);
 }
 
-//float calcKXdeg(float x){
-//    return -310.54*x+156.65;
-//}
-
 void RollAlarm()
 {
     if((roll < -ROLL_L_MAX_DEG ) && (roll > ROLL_L_MAX_DEG-180)) {
@@ -431,26 +412,35 @@
     }
 }
 
+//void updateCadenceFunc(void const *arg)
+//{
+//    while(1) {
+////        VCmonitor.getVoltage(&V) == 0;
+//        updateCadence(V,mgPin.read()*3300.0, mgPin2.read()*3300.0);
+//        Thread::wait(330);
+//    }
+//}
 int main()
 {
     Thread mpu_thread(&mpuProcessing);
-    Thread SD_thread(&SDprintf);
-    bool isFirstCadenceFlag = true;
+    // Thread SD_thread(&SDprintf);
+    //Thread cadenceThread(&updateCadenceFunc);
+    cadenceInter.rise(&updateCadence);
+    cadenceInter.fall(&updateCadence);
 //    Thread soudaSerial_thread(&DataReceiveFromSouda);
     init();
-
+    int counter = 0;
     while(1) {
-        if(VCmonitor.getVoltage(&V) == 0) {
+        if(counter%50== 0) {
+            VCmonitor.getVoltage(&V);
             printf("e:%f\n",V);
         }
-        updateCadence(V,mgPin.read() * 3.3,mgPin2.read() * 3.3,isFirstCadenceFlag);
-        isFirstCadenceFlag = false;
-        //pc.printf("test\n\r");
-//        mpuProcessing();
+        counter++;
+//        updateCadence(V,mgPin.read() * 3.3 * 1000.0,mgPin2.read() * 3.3* 1000.0);
         sonarCalc();
         RollAlarm();
         DataReceiveFromSouda();
         WriteDatas();
-        led4 = !led4;
+        //   wait_ms(20);
     }
 }
\ No newline at end of file