Diff: main.cpp

Revision:

2:391e8bf671ef

Parent:

1:cd11c1c592c7

Child:

3:880d00d555c1

--- a/main.cpp	Tue Jul 19 14:30:51 2016 +0000
+++ b/main.cpp	Sat Jul 30 16:42:10 2016 +0000
@@ -1,20 +1,21 @@
 #include "mbed.h"
 #include "Barometer.h"
-#include "Ultrasonic.h"
 #include "LocalFileSystem.h"
 #include "math.h"
+#include "Servo.h"
 
 Serial pc(USBTX, USBRX);
-int data[14];
+Timer end;
 int stat=1;
 
 ////////////////////////////////////
-//         GPS         5V p27(RX) //
+//         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;
+
+char GPS_msg[150], ns, ew;
+int i = 0, j=0, k=0, gps_ok=0, GPS_flag;
 volatile unsigned char GPS_buffer[2];
 float fix,sat,x,longitude=0.0f,latitude=0.0f, alt_GPS=0,lock, Kor_time;
 
@@ -24,10 +25,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 (GPS_flag == 1){GPS_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++;}
+    if ((i==5)&(GPS_buffer[0] == 'G')){GPS_flag=1;}
+    if (GPS_flag==1){GPS_msg[j]=GPS_buffer[0]; j++;}
 }
 
 float trunc(float v) {
@@ -44,7 +45,7 @@
 {
     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);
+        sscanf(GPS_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);
@@ -57,159 +58,201 @@
     }
 }
 
+//Bluetooth code is placed under the Log_data
+
+////////////////////////////////////////////
+//    Barometer     3.3V p9(SDA) p10(SCL) //  
+////////////////////////////////////////////
+Barometer barometer(p9, p10);
+float t, alt, del_alt;
+float alt_sum=0.0f, alt_zero=0.0f;
+int count = 0, baro_ok = 0;          // for zero-calibration
+float alt_buffer[4], w_alt=0.6;          // weight for LPF
+
+void get_Baro(float*t, float*alt)
+{   
+    if (baro_ok==1){
+    barometer.update();
+    *t = barometer.get_temperature();
+    *alt = barometer.get_altitude_m();
+    baro_ok = 0;
+    }
+} 
+
+void calb_alt(){
+    if (alt==0){}
+    else {
+        if (count<=19){alt_sum = alt_sum + alt; count++;}
+        else {
+            Blue_GPS.printf("calibration\r\n");
+            alt_zero = alt_sum / (float)count;
+            count++;
+        }
+    }
+}
+
+void lpf_alt(){
+    alt_buffer[1] = alt_buffer[0];
+    alt_buffer[0] = alt;
+    alt = w_alt*alt_buffer[1]+(1-w_alt)*alt_buffer[0];  // Low Pass Filter 
+    alt_buffer[3] = alt_buffer[2];
+    alt_buffer[2] = alt;
+    del_alt = alt_buffer[3]-alt_buffer[2];      // for calculation of drop speed
+}
+
 ///////////////////////////////////////
-//         AHRS          5V p14(RX)  //
+//         AHRS          5V p14(RX)  //   20Hz
 ///////////////////////////////////////
 Serial AHRS(p13, p14);
 float roll,pitch,yaw,accx,accy,accz;
-float t, alt;
+char AHRS_msg[150];
+int m=0, ahrs_ok=0, AHRS_flag;
+volatile unsigned char AHRS_buffer[2];
+float roll_buffer[2], pitch_buffer[2], yaw_buffer[2], w_attitude=0.8;
+
+void AHRS_isr(){                      //inturupt 
+    while(AHRS.readable()){
+        AHRS_buffer[1] = AHRS_buffer[0];
+        AHRS_buffer[0] = AHRS.getc();
+        if (AHRS_buffer[0] == '\n' && AHRS_flag == 1){AHRS_flag = 0; ahrs_ok = 1; m=0;}
+        if (AHRS_buffer[0] == '*'){AHRS_flag=1;}
+        if (AHRS_flag==1){AHRS_msg[m] = AHRS_buffer[0]; m++;}
+    }
+} 
 
 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;
+    if (ahrs_ok == 1){
+        ahrs_ok = 0;
+        sscanf(AHRS_msg, "*%f,%f,%f,%f,%f,%f\n", roll, pitch, yaw, accx, accy, accz);
+        baro_ok = 1;
+    }
+}   
 
-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 lpf_attitude(){
+    roll_buffer[1] = roll_buffer[0];
+    roll_buffer[0] = roll;
+    roll = w_attitude*roll_buffer[1]+(1-w_attitude)*roll_buffer[0];  // Low Pass Filter 
+    pitch_buffer[1] = pitch_buffer[0];
+    pitch_buffer[0] = pitch;
+    pitch = w_attitude*pitch_buffer[1]+(1-w_attitude)*pitch_buffer[0];  // Low Pass Filter 
+    yaw_buffer[1] = yaw_buffer[0];
+    yaw_buffer[0] = yaw;
+    yaw = w_attitude*yaw_buffer[1]+(1-w_attitude)*yaw_buffer[0];  // Low Pass Filter 
 }
 
-/*
-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);
+Servo Micro_gf(p22);
 
-///////////////////////////////////
-//           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;
+float unf_value=1.0, gf_value = 0.35,linear_value = 1.0;
+float tg_yaw = 0.0, err_yaw = 0.0, p=1.5;
+void ctl_gf(){
+    err_yaw = yaw - tg_yaw;
+    gf_value = 0.35*exp(0.0039*p*err_yaw);
+    if (err_yaw<0){gf_value = 0.7-0.35*exp(0.0039*p*(-err_yaw));}
+    if (gf_value<=0.0){gf_value=0.0;}
+    else if (gf_value>=0.7) {gf_value=0.7;}
+    Micro_gf = gf_value;
 }
 
 ///////////////////////////////
 //   Datalogger       Mbed   //
 ///////////////////////////////
 LocalFileSystem local("local");
-int num=0;
+int file_no=1;
+char filename[256];
 FILE *fp;
 
+void Log_file(){
+    sprintf(filename, "/local/Data%d.txt", file_no); //save file name for writing
+    fp = fopen(filename, "r"); // if file can be loaded, return is 1
+    while(fp){
+        fclose(fp);
+        file_no ++;
+        sprintf(filename, "/local/Data%d.txt", file_no);    // Open "tem%d.txt" on the local file system for writing
+        fp = fopen(filename, "r");
+    }
+    fp = fopen(filename, "w");    
+}
+
 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");
+    fprintf(fp, "%i,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\r\n",stat,Kor_time,latitude,longitude,alt,alt_GPS,roll,pitch,yaw,accx,accy,accz,unf_value,gf_value,linear_value);
 }
 
+void send_Blue(){
+    Blue_GPS.printf("%i,%.0f,%.3f,%.3f,%.2f,%.2f,%.2,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\n",stat,Kor_time,latitude,longitude,alt,alt_GPS,del_alt,roll,pitch,yaw,accx,accy,accz,unf_value,gf_value,linear_value);
+}
+
+/////////////////////////////////
+//         Main loop           //
+/////////////////////////////////
+
 int main(void)
 {   
-    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");
+    AHRS.attach(&AHRS_isr, Serial::RxIrq);
+    Blue_GPS.printf("Start\r\n");
+    //Log_file();
+    gf_value = 0.7;
+    Micro_gf = gf_value;
         while(1) {
         switch(stat){
-            case 1 :   //Bluetooth connection and calibration
+            case 1 : //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;}
+                calb_alt();
+                lpf_attitude();
+                Blue_GPS.printf("%i %f  %f  %f\r\n",count, alt, alt_sum, alt_zero);
+                if (20<count) {
+                    stat=2;
+                }
                 break;
-            case 2 :   //Waiting launch
-                get_AHRS(&roll,&pitch,&yaw,&accx,&accy,&accz);
-                get_Baro(&t, &alt);
-                alt = alt - alt_zero;
+            case 2 : //Wait
                 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);
+                //Log_data();
                 alt = alt - alt_zero;
+                lpf_alt();
+                lpf_attitude();
+                send_Blue();
+                if (alt<=-10 && abs(accx)<0.1 && abs(accy)<0.1){
+                    //fclose(fp); 
+                    stat=3; send_Blue();
+                }
+                break;
+            /*case 3 :  //Drop
                 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);
+                //Log_data();
+                lpf_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;}
+                lpf_attitude();
+                send_Blue();
+                pc.printf("%i,%.2f,%.2f,%.2f,%.2f,%.2f,%.2,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",stat,Kor_time,latitude,longitude,alt,alt_GPS,del_alt,roll,pitch,yaw,accx,accy,accz,unf_value,gf_value,linear_value);
+                if (alt<=5 && abs(accx)<0.1 && abs(accy)<0.1) {end.start(); stat=4;}
                 break;
-            case 5 :   //Landing
+            case 4 :  //Landing
                 get_AHRS(&roll,&pitch,&yaw,&accx,&accy,&accz);
                 get_Baro(&t, &alt);
+                get_GPS(&Kor_time,&latitude,&ns,&longitude,&ew,&fix,&sat,&x,&alt_GPS,&lock);
+                //Log_data();
+                lpf_alt();
                 alt = alt - alt_zero;
-                get_GPS(&Kor_time,&latitude,&ns,&longitude,&ew,&fix,&sat,&x,&alt_GPS,&lock);
-                packet_data();
+                lpf_attitude();
+                send_Blue();
+                end.read();
+                pc.printf("%i,%.2f,%.2f,%.2f,%.2f,%.2f,%.2,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",stat,Kor_time,latitude,longitude,alt,alt_GPS,del_alt,roll,pitch,yaw,accx,accy,accz,unf_value,gf_value,linear_value);
+                if (end.read()>=10) {fclose(fp);}
+                break;*/
+            case 3 :   //Shut down
                 break;
         }
     }