Diff: main.cpp

Revision:

1:cd11c1c592c7

Parent:

0:6ac6b2d2bf1a

Child:

2:9d0f979369cf

--- a/main.cpp	Fri Jul 08 05:39:27 2016 +0000
+++ b/main.cpp	Tue Jul 19 14:30:51 2016 +0000
@@ -1,37 +1,30 @@
 #include "mbed.h"
 #include "Barometer.h"
 #include "Ultrasonic.h"
+#include "LocalFileSystem.h"
 #include "math.h"
+
 Serial pc(USBTX, USBRX);
-Barometer barometer(p9, p10);
-Ultrasonic sonar(p24, p12); 
-Serial AHRS(p13, p14);
-Serial GPS(p28, p27);
-PwmOut Linear(p21);
-PwmOut Micro_gf(p22);
-PwmOut Micro_d(p23);
+int data[14];
+int stat=1;
 
-float roll,pitch,yaw,accx,accy,accz;
-float p = 0.0f, t = 0.0f, alt = 0.0f;
-int i = 0, j=0, gps_ok=0, flag, lock
-int stat, chksum=0;
+////////////////////////////////////
+//         GPS         5V p27(RX) //
+//      Bluetooth    3.3V p28(TX) //
+////////////////////////////////////
+Serial Blue_GPS(p28, p27);
+char msg[150], ns, ew;
+int i = 0, j=0, gps_ok=0, flag;
 volatile unsigned char GPS_buffer[2];
-char ns, ew;
-char msg[150];
+float fix,sat,x,longitude=0.0f,latitude=0.0f, alt_GPS=0,lock, Kor_time;
 
 void GPS_isr(){
     i++;
     GPS_buffer[1] = GPS_buffer[0];
-    GPS_buffer[0] = GPS.getc();
-    if ((GPS_buffer[1]==13)&(GPS_buffer[0]==10))
-    {   
+    GPS_buffer[0] = Blue_GPS.getc();
+    if ((GPS_buffer[1]==13)&(GPS_buffer[0]==10)){   
         i=0;
-        if (flag == 1)
-        {
-            flag = 0;
-            gps_ok = 1;
-            j=0;
-        }
+        if (flag == 1){flag = 0;gps_ok = 1;j=0;}
     }
     if ((i==5)&(GPS_buffer[0] == 'G')){flag=1;}
     if (flag==1){msg[j]=GPS_buffer[0]; j++;}
@@ -42,48 +35,182 @@
         v*= -1.0;
         v = floor(v);
         v*=-1.0;
-    } else {v = floor(v);}
+    }
+    else {v = floor(v);}
     return v;
 }
 
+void get_GPS(float *Kor_time, float *latitude, char *ns, float *longitude, char *ew, float *fix, float *sat, float *x, float *alt_GPS, float *lock)
+{
+    if (gps_ok == 1){
+        gps_ok = 0;
+        sscanf(msg, "GA,%f,%f,%c,%f,%c,%f,%f,%f,%f,%f", Kor_time,latitude,ns,longitude,ew,fix,sat,x,alt_GPS,lock);
+        if(*ns == 'S') {*latitude  *= -1.0; }
+        if(*ew == 'W') {*longitude *= -1.0; }
+        float degrees = trunc(*latitude / 100.0f);
+        float minutes = (*latitude - (degrees * 100.0f));
+        *latitude = degrees + minutes / 60.0f;    
+        degrees = trunc(*longitude / 100.0f);
+        minutes = *longitude - (degrees * 100.0f);
+        *longitude = degrees + minutes / 60.0f;
+        *Kor_time = *Kor_time + 90000;
+    }
+}
+
+///////////////////////////////////////
+//         AHRS          5V p14(RX)  //
+///////////////////////////////////////
+Serial AHRS(p13, p14);
+float roll,pitch,yaw,accx,accy,accz;
+float t, alt;
+
+void get_AHRS(float*roll, float*pitch, float*yaw, float*accx, float*accy, float*accz)
+{
+    while(AHRS.getc() != '\n');
+    AHRS.scanf("*%f,%f,%f,%f,%f,%f \n", roll, pitch, yaw, accx, accy, accz);
+}       
+   
+////////////////////////////////////////////
+//    Barometer     3.3V p9(SDA) p10(SCL) //
+////////////////////////////////////////////
+Barometer barometer(p9, p10);
+float alt_sum=0.0f, alt_zero=0.0f, alt_avg=0.0f;  // for zero-calibration
+float alt_buffer[5];
+int count = 0;
+
+void get_Baro(float*t, float*alt)
+{
+    barometer.update();
+    *t = barometer.get_temperature();
+    *alt = barometer.get_altitude_m();
+} 
+
+void Baro_zero(){
+    if (count<=4){
+        alt_sum = alt_sum + alt;
+        count++;
+    }
+    else {
+        pc.printf("calibration\r\n");
+        alt_zero = alt_sum / (float)count ;
+    }
+}
+
+/*
+void Baro_check(){
+    alt_buffer[0] = alt;
+    for(i=0;i<=3;i++) {alt_buffer[i+1] = alt_buffer[i]; } //Save in 5 buffers
+    del_alt = alt_min - alt_max;
+}
+*/
+
+///////////////////////////////////
+//      Servo         5V  PWM    //
+///////////////////////////////////
+PwmOut Linear(p21);
+PwmOut Micro_gf(p22);
+PwmOut Micro_unf(p23);
+
+///////////////////////////////////
+//           Checksum            //
+///////////////////////////////////
+long chksum;
+unsigned int checksum (int data, short message_length)
+{
+   unsigned int  sum = 10;
+   /*for (i=0;i<=message_length; i++){
+      sum += data[i];
+   }
+    sum =  (sum & 0xff);*/
+    return (unsigned int)(~sum);
+}
+
+void packet_data(){
+    data[0] = stat;
+    data[1] = Kor_time;
+    data[2] = (int)(alt*100);
+    data[3] = (int)(alt_GPS*100);
+    data[4] = (int)(roll*100);
+    data[5] = (int)(pitch*100);
+    data[6] = (int)(yaw*100);
+    data[7] = (int)(accz*100);
+    data[8] = (int)(latitude*100);
+    data[9] = (int)(longitude*100);
+    data[10] = Micro_unf*100;
+    data[11] = Micro_gf*100;
+    data[12] = Linear*100;
+    data[13] = chksum;
+}
+
+///////////////////////////////
+//   Datalogger       Mbed   //
+///////////////////////////////
+LocalFileSystem local("local");
+int num=0;
+FILE *fp;
+
+void Log_data(){
+    FILE *fp = fopen("/local/data.txt", "w"); 
+    for (num=0;num<=13;num++) {fprintf(fp, "%i ",data[num]);}
+    num=0;
+    fprintf(fp, "\r\n");
+}
+
 int main(void)
-{   AHRS.baud(9600);
-    GPS.baud(9600);
-    GPS.attach(&GPS_isr, Serial::RxIrq);
-    float longitude,latitude, time;
-    Linear.period(0.01); // set PWM period to 1ms
-    Micro_gf.period(0.01);
-    Micro_d.period(0.01);
-    //Linear=0.1;wait(1);
-    Linear=0.19;
-    Micro_d=0.2;
-    Micro_gf=0.2;
-    wait(1);
-    while(1) {
-        //stat should be added
-        barometer.update();
-        p = barometer.get_pressure();
-        t = barometer.get_temperature();
-        alt = barometer.get_altitude_m();
-        while(AHRS.getc() != '\n');
-        AHRS.scanf("*%f,%f,%f,%f,%f,%f \n", &roll, &pitch, &yaw, &accx,&accy,&accz);
-        if (gps_ok == 1)
-        {gps_ok = 0;
-         sscanf(msg, "GA,%f,%f,%c,%f,%c,%d", &time, &latitude, &ns, &longitude, &ew, &lock);
-            if(ns == 'S') {latitude  *= -1.0; }
-            if(ew == 'W') {longitude *= -1.0; }
-            float degrees = trunc(latitude / 100.0f);
-            float minutes = latitude - (degrees * 100.0f);
-            latitude = degrees + minutes / 60.0f;    
-            degrees = trunc(longitude / 100.0f);
-            minutes = longitude - (degrees * 100.0f);
-            longitude = degrees + minutes / 60.0f;
-            time = time + 90000;
+{   
+    wait(10);
+    AHRS.baud(9600);
+    Blue_GPS.baud(9600);
+    Blue_GPS.attach(&GPS_isr, Serial::RxIrq);
+    Linear.period(0.01); Micro_gf.period(0.01); Micro_unf.period(0.01);
+    Linear=0.19; Micro_unf=0.2; Micro_gf=0.2;//Initial setting of servo
+    //Linear=0.1;wait(1);Take back
+    pc.printf("Start\r\n");
+        while(1) {
+        switch(stat){
+            case 1 :   //Bluetooth connection and calibration
+                get_GPS(&Kor_time,&latitude,&ns,&longitude,&ew,&fix,&sat,&x,&alt_GPS,&lock);
+                get_AHRS(&roll,&pitch,&yaw,&accx,&accy,&accz);
+                get_Baro(&t, &alt);
+                Baro_zero();
+                packet_data();
+                //Log_data();
+                pc.printf("%i %f  %f  %f\r\n",count, alt, alt_sum, alt_zero);
+                //if (Kor_time!=0 && abs(accx)<0.1 && abs(accy)<0.1 && abs(accz)<0.1 && 101<=count){stat=2;}
+                if (5<=count){Baro_zero(); pc.printf("%i %f  %f  %f\r\n",count, alt, alt_sum, alt_zero); stat=2;}
+                break;
+            case 2 :   //Waiting launch
+                get_AHRS(&roll,&pitch,&yaw,&accx,&accy,&accz);
+                get_Baro(&t, &alt);
+                alt = alt - alt_zero;
+                get_GPS(&Kor_time,&latitude,&ns,&longitude,&ew,&fix,&sat,&x,&alt_GPS,&lock);
+                packet_data();
+                pc.printf("%i %i %i %i %i %i %i %i %i %i %i %i %i %i %i %i\r\n",data[0],data[1],data[2],data[3],data[4],data[5],data[6],data[7],data[8],data[9],data[10],data[11],data[12],data[13]);
+                if (100.0<alt_GPS && accz<0){stat=3;}
+                break;
+            case 3 :   //Grid fin and landing leg spreading and control
+                get_AHRS(&roll,&pitch,&yaw,&accx,&accy,&accz);
+                get_Baro(&t, &alt);
+                alt = alt - alt_zero;
+                get_GPS(&Kor_time,&latitude,&ns,&longitude,&ew,&fix,&sat,&x,&alt_GPS,&lock);
+                packet_data();
+                if (alt<3){stat=4;}
+                break;
+            case 4 :   //Reverse thrust for soft landing
+                get_AHRS(&roll,&pitch,&yaw,&accx,&accy,&accz);
+                get_Baro(&t, &alt);
+                alt = alt - alt_zero;
+                get_GPS(&Kor_time,&latitude,&ns,&longitude,&ew,&fix,&sat,&x,&alt_GPS,&lock);
+                packet_data();
+                if (accx<0.1 && accy<0.1 && accz<0.1) {stat=5;}
+                break;
+            case 5 :   //Landing
+                get_AHRS(&roll,&pitch,&yaw,&accx,&accy,&accz);
+                get_Baro(&t, &alt);
+                alt = alt - alt_zero;
+                get_GPS(&Kor_time,&latitude,&ns,&longitude,&ew,&fix,&sat,&x,&alt_GPS,&lock);
+                packet_data();
+                break;
         }
-        unsigned int distance = sonar.distance();
-        if (distance<10){Linear =0.1;}
-        chksum=(int)stat+t+alt+roll+pitch+yaw+accz+time+latitude+longitude+distance;
-     pc.printf("\r\n Temperature(c):%f Altitude(m):%f Roll:%f Pitch:%f Yaw:%f Accz:%f Time:%f Latitude:%f Longitude:%f Sonar(cm)%u",stat,t,alt,roll,pitch,yaw,accz,time,latitude,longitude,distance,chksum);
-    }   
-}
- 
\ No newline at end of file
+    }
+}
\ No newline at end of file