Diff: main.cpp

Branch:

Android?????????

Revision:

19:fa3f9ba17af8

Parent:

15:6966299bea4c

Parent:

18:0a0c4277d960

Child:

21:8802034b7810

Child:

24:483dd4e61ead

--- a/main.cpp	Sat Nov 26 01:12:10 2016 +0000
+++ b/main.cpp	Sat Dec 17 09:12:39 2016 +0000
@@ -1,4 +1,5 @@
 //計器プログラム
+
 #include "mbed.h"
 #include "Fusokukei.h"
 #include "MPU6050.h"
@@ -15,7 +16,6 @@
 #define WRITE_DATAS_LOOP_TIME 1
 #define ROLL_R_MAX_DEG 2
 #define ROLL_L_MAX_DEG 2
-#define MPU_DELT_MIN 250
 #define SD_WRITE_NUM 10
 #define INIT_SERVO_PERIOD_MS 20
 
@@ -23,8 +23,8 @@
 //Ticker cadenceTicker;
 
 Serial pc(USBTX,USBRX);
+Serial Android(p13,p14);
 BufferedSoftSerial twe(p9,p10);
-//Serial soudaSerial(p13,p14);
 BufferedSoftSerial soudaSerial(p17,p18);
 //Ticker writeDatasTicker;
 Timer writeTimer;
@@ -34,8 +34,6 @@
 Fusokukei air;
 volatile int air_kaitensu= 0;
 
-float sum = 0;
-uint32_t sumCount = 0;
 MPU6050 mpu6050;
 Timer t;
 Ticker mpu6050Ticker;
@@ -65,10 +63,6 @@
 void call_calcAirSpeed();
 void init();
 void FusokukeiInit();
-void MpuInit();
-void mpuProcessing();
-//void cadenceRead();
-//void cadenceInit();
 void SdInit();
 void DataReceiveFromSouda();
 void WriteDatas();
@@ -90,16 +84,13 @@
 
 void init(){
     twe.baud(19200);
+    Android.baud(9600);
     soudaSerial.baud(9600);
-//    cadenceTicker.attach(&cadenceDataReceive,0.1);
-    //writeTimer.start();
     kisokuServo.period_ms(INIT_SERVO_PERIOD_MS);
     geikakuServo.period_ms(INIT_SERVO_PERIOD_MS);
     FusokukeiInit();
-    MpuInit(); 
+    mpu6050.MPUInit(t); 
     SdInit();
-    //writeDatasTicker.attach(&WriteDatas,1);
-//    soudaSerial.attach(&DataReceiveFromSouda, Serial::RxIrq);
 }
 
 void FusokukeiInit(){
@@ -107,72 +98,10 @@
     FusokukeiTicker.attach(&call_calcAirSpeed, AIR_LOOP_TIME);
 }
 
-void MpuInit(){
-    i2c.frequency(400000);  // use fast (400 kHz) I2C
-    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);
-        mpu6050.MPU6050SelfTest(SelfTest); // Start by performing self test and reporting values
-        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);
-        } else {
-        }
-    } else {
-        //pc.printf("out\n\r"); // Loop forever if communication doesn't happen
-    }   
-}
-
 double calcPulse(int deg){
     return (0.0006+(deg/180.0)*(0.00235-0.00045));
-
 }
 
-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;
-    }
-}
-
-
 void SdInit(){
     mkdir("/sd/mydir", 0777);
     fp = fopen("/sd/mydir/sdtest2.csv", "w");
@@ -185,17 +114,12 @@
 
 void DataReceiveFromSouda(){
     led2 = !led2;
-    //pc.printf("received\n\r");
-//    bool kaigyo=0;
     for(int i = 0; i < SOUDA_DATAS_NUM; i++){
         if(soudaSerial.readable()) {
             soudaDatas[i] = (char)soudaSerial.getc();
             if(soudaDatas[i]==';') i=-1;
-//            else pc.printf("%5d:%3d",i,soudaDatas[i]);
-//            kaigyo =1;
         }else i--;
     }
-//    if(kaigyo) pc.printf("\n\r");
 }
 
 void SDprintf(){
@@ -247,6 +171,10 @@
 //    twe.printf("%f\n\r",cadence.cadence);
 //    pc.printf("%f\n\r",cadence.cadence);
     //pc.printf("\n\r");
+
+if(Android.writeable()){
+    Android.printf("%f,%f,%f",airSpeed,roll,0);
+}
     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); 
@@ -275,7 +203,6 @@
     else{
         RollAlarmL = 0;
     }
-    
     if((roll > 0) && (roll < 180-ROLL_R_MAX_DEG)){
         RollAlarmR = 1;
     }
@@ -285,7 +212,6 @@
 }
 
 void WriteServo(){
-    //kisokuServo.pulsewidth(calcPulse(airSpeed*10));
     kisokuServo.pulsewidth(calcPulse(9*airSpeed));
     if(pitch<0){
         geikakuServo.pulsewidth(calcPulse(0));
@@ -293,19 +219,13 @@
     else{
         geikakuServo.pulsewidth(calcPulse(abs(pitch*90/13.0)));
     }
-    //pc.printf("a:%f",airSpeed);
-    //pc.printf("p:%f\r\n",pitch);
-    //kisokuServo.pulsewidth(calcPulse(0));
-    //geikakuServo.pulsewidth(calcPulse(0));
 }
 
 int main(){
     init();
     while(1){
         pc.printf("test\n\r");
-//        if(test.readable()) pc.printf("%d   ",test.getc());
-//        pc.printf("<-softserial\n\r");
-        mpuProcessing();
+        mpu6050.mpuProcessing(t);
         RollAlarm();
         DataReceiveFromSouda();
 //        cadence.readData();