Diff: main.cpp

Revision:

3:e3e965924dde

Parent:

2:9d0f979369cf

Child:

4:b2eaf0cf5063

--- a/main.cpp	Wed Jul 27 16:17:26 2016 +0000
+++ b/main.cpp	Tue Aug 02 14:38:15 2016 +0000
@@ -4,7 +4,7 @@
 #include "math.h"
 #include "Servo.h"
 
-Serial pc(USBTX, USBRX);
+Ticker blue_trig;
 Timer end;
 int stat=1;
 
@@ -17,6 +17,7 @@
 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;
+int blue_ok=0;
 
 void GPS_isr(){
     i++;
@@ -57,6 +58,21 @@
     }
 }
 
+void blue_trig_isr(){
+    blue_ok=1;   // AHRS 20HZ
+}
+
+void trans_blue_data(float in_data, int integer_point, int under_point){  // number of intefer and under_point
+        unsigned int conv_trans_data;
+        conv_trans_data = (unsigned int)abs(in_data * pow((float)10,under_point));
+        if(in_data<0) {while((LPC_UART2->LSR&0x20)==0); LPC_UART2->THR = '-';}
+        else {while((LPC_UART2->LSR&0x20)==0); LPC_UART2->THR = '+';}
+        for(int cnt_num=(integer_point + under_point); cnt_num > 0; cnt_num--){
+            if(cnt_num == under_point) {while((LPC_UART2->LSR&0x20)==0); LPC_UART2->THR = '.';   }        
+            while((LPC_UART2->LSR&0x20)==0); LPC_UART2->THR = (unsigned char)(conv_trans_data%(unsigned int)(pow((float)10,cnt_num))/(pow((float)10,cnt_num-1)) + 48);       //convert to ASCII    
+        }
+}
+
 //Bluetooth code is placed under the Log_data
 
 ////////////////////////////////////////////
@@ -66,48 +82,54 @@
 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.4;          // weight for LPF
+float alt_buffer[4], w_alt=0.6;          // weight for LPF
 
-void get_Baro(float*t, float*alt)
+void get_Baro(float*alt, float*del_alt)
 {   
     if (baro_ok==1){
-    barometer.update();
-    *t = barometer.get_temperature();
-    *alt = barometer.get_altitude_m();
-    baro_ok = 0;
+        barometer.update();
+        *alt = barometer.get_altitude_m();
+        alt_buffer[1] = alt_buffer[0]; 
+        alt_buffer[0] = *alt;
+        if(abs(alt_buffer[0]- alt_buffer[1])>100){
+            alt_buffer[0] = alt_buffer[1];
+            *alt = w_alt*alt_buffer[1]+(1-w_alt)*alt_buffer[0];  // Low Pass Filter 
+            *del_alt = 0;      // for calculation of drop speed
+            baro_ok = 0;
+        }
+        else{
+            *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
+            baro_ok = 0;
+        }
     }
 } 
 
 void calb_alt(){
-    if (alt==0){}
+    if (alt==0){count=0;}
     else {
-        if (count<=99){alt_sum = alt_sum + alt; count++;}
-        else {
-            Blue_GPS.printf("calibration\r\n");
-            alt_zero = alt_sum / (float)count;
-            count++;
+        if (count==1){count++;}
+        else{
+            if (count<=99){alt_sum = alt_sum + alt; count++;}
+            else {
+                alt_zero = alt_sum / (float)(count-2);
+                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)  //   20Hz
 ///////////////////////////////////////
 Serial AHRS(p13, p14);
 float roll,pitch,yaw,accx,accy,accz;
 char AHRS_msg[150];
-int m=0, ahrs_ok=0, AHRS_flag;
+int m=0, ahrs_ok=0, AHRS_flag=0;
 volatile unsigned char AHRS_buffer[2];
-float roll_buffer[2], pitch_buffer[2], yaw_buffer[2], w_attitude=0.8;
+float roll_buffer[2], pitch_buffer[2], yaw_buffer[2], w_attitude=0.7;
 
 void AHRS_isr(){                      //inturupt 
     while(AHRS.readable()){
@@ -124,21 +146,18 @@
     if (ahrs_ok == 1){
         ahrs_ok = 0;
         sscanf(AHRS_msg, "*%f,%f,%f,%f,%f,%f\n", roll, pitch, yaw, accx, accy, accz);
+        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 
         baro_ok = 1;
     }
-}   
-
-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 
-}
+} 
 
 ///////////////////////////////////
 //      Servo         5V  PWM    //
@@ -147,7 +166,7 @@
 Servo Micro_gf(p22);
 Servo Linear(p23);
 
-float unf_value = 1.0, gf_value = 0.35, linear_value = 0.5;
+float unf_value=1.0, gf_value = 0.35, linear_value = 0.25;
 float tg_yaw = 0.0, err_yaw = 0.0, p=1.5;
 
 void ctl_gf(){
@@ -159,39 +178,6 @@
     Micro_gf = gf_value;
 }
 
-///////////////////////////////////
-//           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);
-}
-/*
-float data[15];
-void packet_data(){
-    data[0] = stat;
-    data[1] = Kor_time;
-    data[2] = alt;
-    data[3] = alt_GPS;
-    data[4] = roll;
-    data[5] = pitch;
-    data[6] = yaw;
-    data[7] = accz;
-    data[8] = latitude;
-    data[9] = longitude;
-    data[10] = unf_value;
-    data[11] = gf_value;
-    data[12] = linear_value;
-    data[13] = chksum;
-}
-*/
 ///////////////////////////////
 //   Datalogger       Mbed   //
 ///////////////////////////////
@@ -213,49 +199,77 @@
 }
 
 void Log_data(){
-    fprintf(fp, "%i,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",stat,Kor_time,latitude,longitude,alt,alt_GPS,roll,pitch,yaw,accx,accy,accz,unf_value,gf_value,linear_value);
+    fprintf(fp, "%i,%.0f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%.2f,%.2f,%.2f\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,%.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 (blue_ok == 1){
+        blue_ok = 0;
+        trans_blue_data((float)stat,1,0);
+        while((LPC_UART2->LSR&0x20)==0); LPC_UART2->THR = ','; // basic formation of way to send data
+        trans_blue_data(Kor_time,6,0);
+        while((LPC_UART2->LSR&0x20)==0); LPC_UART2->THR = ',';
+        trans_blue_data(latitude,2,6);
+        while((LPC_UART2->LSR&0x20)==0); LPC_UART2->THR = ','; 
+        trans_blue_data(longitude,3,6);
+        while((LPC_UART2->LSR&0x20)==0); LPC_UART2->THR = ',';
+        trans_blue_data(alt,3,2);
+        while((LPC_UART2->LSR&0x20)==0); LPC_UART2->THR = ',';
+        trans_blue_data(del_alt,3,2);
+        while((LPC_UART2->LSR&0x20)==0); LPC_UART2->THR = ',';
+        trans_blue_data(alt_GPS,3,2);
+        while((LPC_UART2->LSR&0x20)==0); LPC_UART2->THR = ',';
+        trans_blue_data(roll,3,2);
+        while((LPC_UART2->LSR&0x20)==0); LPC_UART2->THR = ',';
+        trans_blue_data(pitch,3,2);
+        while((LPC_UART2->LSR&0x20)==0); LPC_UART2->THR = ',';
+        trans_blue_data(yaw,3,2);
+        while((LPC_UART2->LSR&0x20)==0); LPC_UART2->THR = ',';
+        trans_blue_data(accz,2,2);
+        while((LPC_UART2->LSR&0x20)==0); LPC_UART2->THR = ',';
+        trans_blue_data(unf_value,1,2);
+        while((LPC_UART2->LSR&0x20)==0); LPC_UART2->THR = ',';
+        trans_blue_data(gf_value,1,2);
+        while((LPC_UART2->LSR&0x20)==0); LPC_UART2->THR = ',';
+        trans_blue_data(linear_value,1,2);
+        while((LPC_UART2->LSR&0x20)==0); LPC_UART2->THR = '\n';
+    }
 }
 
+
 /////////////////////////////////
-//         Main loop           //
+//         Main loop           // 
 /////////////////////////////////
 
 int main(void)
 {   
     AHRS.baud(9600);
     Blue_GPS.baud(9600);
-    Blue_GPS.attach(&GPS_isr, Serial::RxIrq);
-    AHRS.attach(&AHRS_isr, Serial::RxIrq);
-    Micro_unf = unf_value; //Initial setting of servo
-    pc.printf("Start\r\n");
+    Blue_GPS.attach(&GPS_isr);
+    AHRS.attach(&AHRS_isr);
+    blue_trig.attach(&blue_trig_isr, 0.125); 
+    Micro_gf = gf_value; //Initial setting of servo
         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);
+                get_Baro(&alt, &del_alt);
                 calb_alt();
-                lpf_attitude();
                 send_Blue();
-                Blue_GPS.printf("%i %f  %f  %f\r\n",count, alt, alt_sum, alt_zero);
-                if (Kor_time!=0.0 && abs(accx)<0.1 && abs(accy)<0.1 && abs(accz)<0.1 && 101<=count){stat=2;}
+                if (100<count) {stat=2;}
                 break;
-            case 2 :   //Waiting launch
+            case 2 :   //Wait for launch and keep the order get_AHRS, it sets trigger of get_Baro and send_Blue and Log-data save raw_alt
                 get_GPS(&Kor_time,&latitude,&ns,&longitude,&ew,&fix,&sat,&x,&alt_GPS,&lock);
                 get_AHRS(&roll,&pitch,&yaw,&accx,&accy,&accz);
-                lpf_attitude();
-                get_Baro(&t, &alt);
+                get_Baro(&alt, &del_alt);
                 alt = alt - alt_zero;
                 send_Blue();
                 if (100.0<alt_GPS && del_alt>1){
                     stat=3;
                     Log_file();
-                    unf_value=0.0;
                     Micro_unf = unf_value;
+                    unf_value = 0.0;   //checking
                     wait(0.5);
                     Micro_gf = gf_value;
                 }
@@ -263,36 +277,44 @@
             case 3 :   //Grid fin and landing leg spreading and control
                 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);
+                get_Baro(&alt, &del_alt);
                 Log_data();
-                lpf_alt();
                 alt = alt - alt_zero;
-                lpf_attitude();
                 send_Blue();
-                if (alt<10){stat=4; Linear = linear_value;} // need to modify trigger 
+                if (alt<10){ // need to modify trigger 
+                    fclose(fp);
+                    Log_file();
+                    stat=4;
+                    Linear=linear_value;
+                    linear_value=0.0;
+                } 
                 break;
             case 4 :   //Reverse thrust for soft landing
+                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);
-                get_GPS(&Kor_time,&latitude,&ns,&longitude,&ew,&fix,&sat,&x,&alt_GPS,&lock);
+                get_Baro(&alt, &del_alt);
                 Log_data();
-                lpf_alt();
                 alt = alt - alt_zero;
-                lpf_attitude();
                 send_Blue();
-                if (accx<0.1 && accy<0.1 && alt<3) {end.start(); stat=5;}
+                if (accx<0.1 && accy<0.1 && alt<3){
+                    fclose(fp);
+                    Log_file();
+                    end.start();
+                    stat=5;
+                }
                 break;
             case 5 :   //Landing
+                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);
-                get_GPS(&Kor_time,&latitude,&ns,&longitude,&ew,&fix,&sat,&x,&alt_GPS,&lock);
+                get_Baro(&alt, &del_alt);
                 Log_data();
-                lpf_alt();
                 alt = alt - alt_zero;
-                lpf_attitude();
                 send_Blue();
                 end.read();
-                if (end.read()>=10) {fclose(fp); stat=6;}
+                if (end.read()>=10) {
+                    fclose(fp);
+                    stat=6;
+                }
                 break;
             case 6 :   //Shut down
                 break;