Revision 3:8dc516be2e7e, committed 2016-03-24

Comitter:

taurin

Date:

Thu Mar 24 06:42:22 2016 +0000

Parent:

2:5a1a638f5fe6

Child:

4:a863a092141c

Commit message:

3/24 ??; ?;

Changed in this revision

--- a/main.cpp	Thu Feb 25 09:13:46 2016 +0000
+++ b/main.cpp	Thu Mar 24 06:42:22 2016 +0000
@@ -7,17 +7,20 @@
 #define KX_VALUE_MIN 0.4
 #define KX_VALUE_MAX 0.8
 #define SOUDA_DATAS_NUM 13
-#define WRITE_DATAS_NUM 28
+#define WRITE_DATAS_NUM 21
 #define MPU_LOOP_TIME 0.01
 #define AIR_LOOP_TIME 0.01
-#define WRITE_DATAS_LOOP_TIME 0.5
-#define ROLL_R_MAX_DEG 5
-#define ROLL_L_MAX_DEG 5
+#define WRITE_DATAS_LOOP_TIME 1
+#define ROLL_R_MAX_DEG 2
+#define ROLL_L_MAX_DEG 2
+#define MPU_DELT_MIN 500
+#define SD_WRITE_NUM 10
 
 Serial pc(USBTX,USBRX);
 Serial soudaSerial(p13,p14);
 Serial twe(p9,p10);
 Ticker writeDatasTicker;
+Timer writeTimer;
 
 InterruptIn FusokukeiPin(p30);
 Ticker FusokukeiTicker;
@@ -33,6 +36,7 @@
 AnalogIn kx_X(p17);
 AnalogIn kx_Y(p16);
 AnalogIn kx_Z(p15);
+float KX_X,KX_Y,KX_Z;
 
 DigitalOut RollAlarmR(p20);
 DigitalOut RollAlarmL(p19);
@@ -43,7 +47,8 @@
 FILE* fp;
 
 char soudaDatas[SOUDA_DATAS_NUM];
-char writeDatas[WRITE_DATAS_NUM];
+float writeDatas[SD_WRITE_NUM][WRITE_DATAS_NUM];
+volatile int write_datas_index = 0;
 
 void air_countUp();
 void call_calcAirSpeed();
@@ -68,10 +73,11 @@
 
 void init(){
     twe.baud(115200);
+    writeTimer.start();
     FusokukeiInit();
     MpuInit(); 
     SdInit();
-    writeDatasTicker.attach(&WriteDatas,0.25);
+    //writeDatasTicker.attach(&WriteDatas,1);
     //soudaSerial.attach(&DataReceiveFromSouda, Serial::RxIrq);
 }
 
@@ -102,41 +108,42 @@
 
 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 > 800) { // update LCD once per half-second independent of read rate
-            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;
+        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;
+        pc.printf("%f,%f,%f\n\r",pitch,roll,yaw);
+        myled= !myled;
+        count = t.read_ms();
+        sum = 0;
+        sumCount = 0;
     }
 }
 
@@ -158,31 +165,69 @@
     }
 }
 
-void WriteDatas(){
-    pc.printf("write\n\r");
-    for(int i = 0; i < SOUDA_DATAS_NUM; i++){
-        pc.printf("%i   ",soudaDatas[i]);
-        twe.printf("%i  ",soudaDatas[i]);
-    }
-    pc.printf("\n\r");
-    twe.printf("\n\r");
-    twe.printf("%f,%f,%f\n\r",pitch,roll,yaw);
-   // twe.printf("%f,%f,%f\n\r",gx,gy,gz);   
-//    twe.printf("%f\n\r",airSpeed); 
-    pc.printf("%f,%f,%f\n\r",pitch,roll,yaw);
-    pc.printf("%f,%f,%f\n\r",gx,gy,gz);   
-    pc.printf("%f\n\r",airSpeed);
+void SDprintf(){
     fp = fopen("/sd/mydir/sdtest.csv", "a");
     if(fp == NULL) {
         error("Could not open file for write\n");
     }
-    for(int i = 0; i < SOUDA_DATAS_NUM; i++){
-        fprintf(fp,"%i   ",soudaDatas[i]);
+    for(int i = 0; i < SD_WRITE_NUM; i++){
+        for(int j = 0; j < WRITE_DATAS_NUM; j++){
+            fprintf(fp,"%f   ", writeDatas[i][j]);
+            if(i%7==0){
+                fprintf(fp,"\n\r");
+            }
+        }
+    }
+    //for(int i = 0; i < SOUDA_DATAS_NUM; i++){
+//        fprintf(fp,"%i   ",soudaDatas[i]);
+//    }
+//    fprintf(fp, "p:%f,r:%f,y:%f\n",pitch,roll,yaw);
+//    fprintf(fp, "gx:%f,gy:%f,gz:%f\n",calcKXdeg(kx_X.read()),calcKXdeg(KX_Y),calcKXdeg(KX_Z));
+//    fprintf(fp, "as:%f\n",airSpeed);
+    fclose(fp);
+}
+
+void WriteDatas(){
+    int i;
+    for(i = 0; i < SOUDA_DATAS_NUM; i++){
+        writeDatas[write_datas_index][i] = (float)soudaDatas[i];
     }
-    fprintf(fp, "p:%f,r:%f,y:%f\n",pitch,roll,yaw);
-    fprintf(fp, "gx:%f,gy:%f,gz:%f\n",gx,gy,gz);
-    fprintf(fp, "as:%f\n",airSpeed);
-    fclose(fp);
+    writeDatas[write_datas_index][i++] = calcKXdeg(kx_X.read());
+    writeDatas[write_datas_index][i++] = calcKXdeg(kx_Y.read());
+    writeDatas[write_datas_index][i++] = calcKXdeg(kx_Z.read());
+    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++] = writeTimer.read();
+    for(i = 0; i < WRITE_DATAS_NUM; i++){
+        pc.printf("%f   ", writeDatas[write_datas_index][i]);
+        twe.printf("%f   ", writeDatas[write_datas_index][i]);
+        if(i%7==0){
+            pc.printf("\n\r");
+            twe.printf("\n\r");
+        }
+    }
+    if(write_datas_index == SD_WRITE_NUM){
+        SDprintf();
+        write_datas_index=0;
+    }
+    else{
+        write_datas_index++;
+    } 
+        //for(int i = 0; i < SOUDA_DATAS_NUM; i++){
+//            pc.printf("%i   ",soudaDatas[i]);
+//            twe.printf("%i  ",soudaDatas[i]);
+//        }
+//        pc.printf("\n\r");
+//        twe.printf("\n\r");
+//        twe.printf("%f,%f,%f\n\r",pitch,roll,yaw);
+//        twe.printf("%f,%f,%f\n\r",calcKXdeg(kx_X.read()),calcKXdeg(KX_Y),calcKXdeg(KX_Z));   
+//        twe.printf("%f\n\r",airSpeed); 
+//        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);
+//        SDprintf();
 }
 
 float calcKXdeg(float x){
@@ -193,12 +238,6 @@
     return pitch-calcKXdeg(kx_Z.read());
 }
 
-void kxRead(){
-    gx = kx_X.read();
-    gy = kx_Y.read();
-    gz = kx_Z.read();
-}
-
 void RollAlarm(){ 
     if((roll < 0) && (roll > ROLL_L_MAX_DEG-180)){
         RollAlarmL = 1;
@@ -219,8 +258,8 @@
     init();
     while(1){
         mpuProcessing();
-        kxRead();
         RollAlarm();
         DataReceiveFromSouda();
+        WriteDatas();
     }
 }
\ No newline at end of file