Diff: main.cpp

Revision:

1:fc5988ebfa53

Parent:

0:4013a9855dc8

Child:

2:b88f73d7073c

--- a/main.cpp	Wed Aug 17 04:06:17 2016 +0000
+++ b/main.cpp	Wed Aug 17 06:40:34 2016 +0000
@@ -101,6 +101,8 @@
 //pwmをサーボに出力する関数。
 void setall_servo()
 {
+    
+    
     //if(failsafe_status == SBUS_SIGNAL_OK){
     servo1.pulsewidth_us(pwm[0]);
     servo2.pulsewidth_us(pwm[1]);
@@ -109,7 +111,7 @@
     servo5.pulsewidth_us(pwm[4]);
     servo6.pulsewidth_us(pwm[5]);
     servo7.pulsewidth_us(pwm[6]);
-    servo8.pulsewidth_us(pwm[7]);
+    servo8.pulsewidth_us(pwm[7]);   
       /*  }else{
         servo1.pulsewidth_us(1000);
         servo2.pulsewidth_us(1000);
@@ -120,13 +122,13 @@
         servo7.pulsewidth_us(1000);
         servo8.pulsewidth_us(1000);
     }*/
-    
+/*
     for(j=0;j<8;j++){
         pc.printf("ch%d=%d ", j+1, channels[j]);
     }
     //pc.printf("time = %d", frq.read_ms()); 
     pc.printf("\r\n");
-    
+*/
     //frq.reset();
 }
 
@@ -278,7 +280,7 @@
     // Set Datalenght & Frame
     sbus_.format(8, Serial::Even, 2);
     //start sbus irq
-    sbus_.attach(&catch_sbus, RawSerial::RxIrq);
+    //sbus_.attach(&catch_sbus, RawSerial::RxIrq);
     }
 
 //---CheckSW_up---
@@ -340,15 +342,15 @@
     mpu9250.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f,  my,  mx, mz);
     mpu9250.MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
 
-    /*  Auto_Loop()で50ms待ってるから要らないと思う
+    //  Auto_Loop()で50ms待ってるから要らないと思う
     // Serial print and/or display at 0.5 s rate independent of data rates
     delt_t = t.read_ms() - count;
     if (delt_t > 500) { // update LCD once per half-second independent of read rate
-    */
+    
 
-    pc.printf("ax = %f", 1000*ax); 
-    pc.printf(" ay = %f", 1000*ay); 
-    pc.printf(" az = %f  mg\n\r", 1000*az); 
+    //pc.printf("ax = %f", 1000*ax); 
+    //pc.printf(" ay = %f", 1000*ay); 
+    //pc.printf(" az = %f  mg\n\r", 1000*az); 
 
     //pc.printf("gx = %f", gx); 
     //pc.printf(" gy = %f", gy); 
@@ -385,13 +387,13 @@
     yaw   -= 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
     roll  *= 180.0f / PI;
 
-    pc.printf("Yaw,   Pitch,   Roll:  %f  %f  %f \n\r",  yaw,   pitch,   roll);
+    pc.printf("Yaw: %f  Pitch:%f  Roll:%f\n\r",  yaw,   pitch,   roll);
     //pc.printf("average rate = %f\n\r", (float) sumCount/sum);
  
     count = t.read_ms(); 
     sum = 0;
     sumCount = 0; 
-    //}
+    }
     
 }
     
@@ -623,7 +625,7 @@
   
     if (whoami == 0x71){ // WHO_AM_I should always be 0x68
         pc.printf("MPU9250 is online...\n\r");
-        wait(1);
+        //wait(1);
      
         mpu9250.resetMPU9250(); // Reset registers to default in preparation for device calibration
         mpu9250.calibrateMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers  
@@ -633,7 +635,7 @@
         //pc.printf("x accel bias = %f\n\r", accelBias[0]);
         //pc.printf("y accel bias = %f\n\r", accelBias[1]);
         //pc.printf("z accel bias = %f\n\r", accelBias[2]);
-        wait(2);
+        wait(1);
         
         mpu9250.initMPU9250(); 
         pc.printf("MPU9250 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
@@ -669,6 +671,8 @@
           
     led1 = 0;
     led2 = 0;
+    led3 = 0;
+    led4 = 0;
     
     pc.printf("All initialized\r\n");
     
@@ -679,7 +683,7 @@
         //pc.printf("ch%d=%d ", i+1, channels[i]);
         //}
         //pc.printf("\n");
-        //wait_ms(500); 
+        wait_ms(500); 
         
         
         switch(OperationMode){      //変数OperationModeの値で場合分け