Diff: SPATIAL/SPATIAL.cpp

Revision:

1:9530746906b6

Parent:

0:3473b92e991e

--- a/SPATIAL/SPATIAL.cpp	Tue Jun 12 01:05:50 2018 +0000
+++ b/SPATIAL/SPATIAL.cpp	Wed Nov 28 13:06:23 2018 +0000
@@ -7,6 +7,18 @@
 
 BufferedSerial SPATIAL_UART(p28, p27);      // tx, rx
 
+/* ****** 100Hz Receive ********** */
+// Packet ID : 42, Length : 12  --> Angural Velocity X,Y,Z (rad/s)
+// Packet ID : 39, Length : 12  --> Roll, Pitch, Yaw (rad)
+
+/* ****** 10Hz Receive *********** */
+// Packet ID : 35, Length = 12  --> NED Velocity X, Y, Z (m/s)
+// Packet ID : 32, Length = 24  --> System State Packet
+
+/* ****** 1Hz Receive *********** */
+// Packet ID : 22, Length = 14  --> Time Packet
+// Packet ID : 30, Length = 13  --> Satellites Packet
+
 //float 이랑 double 값뽑을때 쓰는거
 union FP8
 {
@@ -30,7 +42,12 @@
     return crc;
 }
 
-void SPATIAL::Init(){
+void SPATIAL::Init(int sensor_wait_input){
+    if( sensor_wait_input == 1){
+        SENSOR_WAIT_BOOL = true;
+    }else{
+        SENSOR_WAIT_BOOL = false;
+    }
     SPATIAL_UART.baud(115200);   
 }
 //이것은 SPATIAL에서 전송되는 각종 데이터를 받는 부분이다.
@@ -39,142 +56,281 @@
   while(SPATIAL_UART.readable() > 0) 
   {
     SPATIAL_ID[4] = SPATIAL_ID[3]; SPATIAL_ID[3] = SPATIAL_ID[2]; SPATIAL_ID[2] = SPATIAL_ID[1]; SPATIAL_ID[1] = SPATIAL_ID[0]; SPATIAL_ID[0] = SPATIAL_UART.getc();
-    if(SPATIAL_ID_20_BOOL == true){
-      SPATIAL_BUF[SPATIAL_BUF_CNT] = SPATIAL_ID[0];
-      if(SPATIAL_BUF_CNT >= 100){ SPATIAL_ID_20_BOOL = false; SPATIAL_ID_20_PARSING_BOOL = true;}
+    if((SPATIAL_ID_39_BOOL == true)|(SPATIAL_ID_42_BOOL == true)|(SPATIAL_ID_35_BOOL == true)|(SPATIAL_ID_32_BOOL == true)|(SPATIAL_ID_22_BOOL == true)|(SPATIAL_ID_30_BOOL == true)){
+//      SPATIAL_BUF[SPATIAL_BUF_CNT] = SPATIAL_ID[0];
+      if(SPATIAL_ID_39_BOOL == true) {
+          SPATIAL_39_BUF[SPATIAL_BUF_CNT] = SPATIAL_ID[0];
+          if(SPATIAL_BUF_CNT >= 12){ SPATIAL_ID_39_BOOL = false; SPATIAL_ID_39_PARSING_BOOL = true;}
+      }else if(SPATIAL_ID_42_BOOL == true) {
+          SPATIAL_42_BUF[SPATIAL_BUF_CNT] = SPATIAL_ID[0];
+          if(SPATIAL_BUF_CNT >= 12){ SPATIAL_ID_42_BOOL = false; SPATIAL_ID_42_PARSING_BOOL = true;} 
+      }else if(SPATIAL_ID_35_BOOL == true) {
+          SPATIAL_35_BUF[SPATIAL_BUF_CNT] = SPATIAL_ID[0];
+          if(SPATIAL_BUF_CNT >= 12){ SPATIAL_ID_35_BOOL = false; SPATIAL_ID_35_PARSING_BOOL = true;} 
+      }else if(SPATIAL_ID_32_BOOL == true) {
+          SPATIAL_32_BUF[SPATIAL_BUF_CNT] = SPATIAL_ID[0];
+          if(SPATIAL_BUF_CNT >= 24){ SPATIAL_ID_32_BOOL = false; SPATIAL_ID_32_PARSING_BOOL = true;} 
+      }else if(SPATIAL_ID_22_BOOL == true) {
+          SPATIAL_22_BUF[SPATIAL_BUF_CNT] = SPATIAL_ID[0];
+          if(SPATIAL_BUF_CNT >= 14){ SPATIAL_ID_22_BOOL = false; SPATIAL_ID_22_PARSING_BOOL = true;} 
+      }else if(SPATIAL_ID_30_BOOL == true) {
+          SPATIAL_30_BUF[SPATIAL_BUF_CNT] = SPATIAL_ID[0];
+          if(SPATIAL_BUF_CNT >= 13){ SPATIAL_ID_30_BOOL = false; SPATIAL_ID_30_PARSING_BOOL = true;} 
+      }
       SPATIAL_BUF_CNT ++;
     }
     else{
-      if((SPATIAL_ID[3] == 20) & (SPATIAL_ID[2] == 100))    //두번째 데이터가 '패킷넘버', 세번째 데이터가 '데이터랭쓰'임... 별도의 SOF가 없어서 이걸 기준으로함.
+      if((SPATIAL_ID[3] == 39) & (SPATIAL_ID[2] == 12))    //두번째 데이터가 '패킷넘버', 세번째 데이터가 '데이터랭쓰'임... 별도의 SOF가 없어서 이걸 기준으로함.
+      {
+        //첫 번째 데이터 : head LRC이며 계산식 돌린거랑 첫번째 데이터랑 일치하면 이 배열을 쓸거라는 의미.
+        if((uint8_t)((((uint32_t)SPATIAL_ID[3] + (uint32_t)SPATIAL_ID[2] + (uint32_t)SPATIAL_ID[1] + (uint32_t)SPATIAL_ID[0]) ^ 0xFF) + 1) == (uint8_t)SPATIAL_ID[4])
+        {
+          SPATIAL_ID_39_BOOL = true;
+          CRC_INPUT = (uint16_t)SPATIAL_ID[0]<<8 | (uint16_t)SPATIAL_ID[1];
+          SPATIAL_BUF_CNT = 0;
+        }
+      }        
+      else if((SPATIAL_ID[3] == 42) & (SPATIAL_ID[2] == 12))    //두번째 데이터가 '패킷넘버', 세번째 데이터가 '데이터랭쓰'임... 별도의 SOF가 없어서 이걸 기준으로함.
       {
         //첫 번째 데이터 : head LRC이며 계산식 돌린거랑 첫번째 데이터랑 일치하면 이 배열을 쓸거라는 의미.
         if((uint8_t)((((uint32_t)SPATIAL_ID[3] + (uint32_t)SPATIAL_ID[2] + (uint32_t)SPATIAL_ID[1] + (uint32_t)SPATIAL_ID[0]) ^ 0xFF) + 1) == (uint8_t)SPATIAL_ID[4])
         {
-          SPATIAL_ID_20_BOOL = true;
+          SPATIAL_ID_42_BOOL = true;
+          CRC_INPUT = (uint16_t)SPATIAL_ID[0]<<8 | (uint16_t)SPATIAL_ID[1];
+          SPATIAL_BUF_CNT = 0;
+        }
+      }        
+      else if((SPATIAL_ID[3] == 35) & (SPATIAL_ID[2] == 12))    //두번째 데이터가 '패킷넘버', 세번째 데이터가 '데이터랭쓰'임... 별도의 SOF가 없어서 이걸 기준으로함.
+      {
+        //첫 번째 데이터 : head LRC이며 계산식 돌린거랑 첫번째 데이터랑 일치하면 이 배열을 쓸거라는 의미.
+        if((uint8_t)((((uint32_t)SPATIAL_ID[3] + (uint32_t)SPATIAL_ID[2] + (uint32_t)SPATIAL_ID[1] + (uint32_t)SPATIAL_ID[0]) ^ 0xFF) + 1) == (uint8_t)SPATIAL_ID[4])
+        {
+          SPATIAL_ID_35_BOOL = true;
+          CRC_INPUT = (uint16_t)SPATIAL_ID[0]<<8 | (uint16_t)SPATIAL_ID[1];
+          SPATIAL_BUF_CNT = 0;
+        }
+      }        
+      else if((SPATIAL_ID[3] == 32) & (SPATIAL_ID[2] == 24))    //두번째 데이터가 '패킷넘버', 세번째 데이터가 '데이터랭쓰'임... 별도의 SOF가 없어서 이걸 기준으로함.
+      {
+        //첫 번째 데이터 : head LRC이며 계산식 돌린거랑 첫번째 데이터랑 일치하면 이 배열을 쓸거라는 의미.
+        if((uint8_t)((((uint32_t)SPATIAL_ID[3] + (uint32_t)SPATIAL_ID[2] + (uint32_t)SPATIAL_ID[1] + (uint32_t)SPATIAL_ID[0]) ^ 0xFF) + 1) == (uint8_t)SPATIAL_ID[4])
+        {
+          SPATIAL_ID_32_BOOL = true;
+          CRC_INPUT = (uint16_t)SPATIAL_ID[0]<<8 | (uint16_t)SPATIAL_ID[1];
+          SPATIAL_BUF_CNT = 0;
+        }
+      }        
+      else if((SPATIAL_ID[3] == 22) & (SPATIAL_ID[2] == 14))    //두번째 데이터가 '패킷넘버', 세번째 데이터가 '데이터랭쓰'임... 별도의 SOF가 없어서 이걸 기준으로함.
+      {
+        //첫 번째 데이터 : head LRC이며 계산식 돌린거랑 첫번째 데이터랑 일치하면 이 배열을 쓸거라는 의미.
+        if((uint8_t)((((uint32_t)SPATIAL_ID[3] + (uint32_t)SPATIAL_ID[2] + (uint32_t)SPATIAL_ID[1] + (uint32_t)SPATIAL_ID[0]) ^ 0xFF) + 1) == (uint8_t)SPATIAL_ID[4])
+        {
+          SPATIAL_ID_22_BOOL = true;
+          CRC_INPUT = (uint16_t)SPATIAL_ID[0]<<8 | (uint16_t)SPATIAL_ID[1];
+          SPATIAL_BUF_CNT = 0;
+        }
+      }        
+      else if((SPATIAL_ID[3] == 30) & (SPATIAL_ID[2] == 13))    //두번째 데이터가 '패킷넘버', 세번째 데이터가 '데이터랭쓰'임... 별도의 SOF가 없어서 이걸 기준으로함.
+      {
+        //첫 번째 데이터 : head LRC이며 계산식 돌린거랑 첫번째 데이터랑 일치하면 이 배열을 쓸거라는 의미.
+        if((uint8_t)((((uint32_t)SPATIAL_ID[3] + (uint32_t)SPATIAL_ID[2] + (uint32_t)SPATIAL_ID[1] + (uint32_t)SPATIAL_ID[0]) ^ 0xFF) + 1) == (uint8_t)SPATIAL_ID[4])
+        {
+          SPATIAL_ID_30_BOOL = true;
           CRC_INPUT = (uint16_t)SPATIAL_ID[0]<<8 | (uint16_t)SPATIAL_ID[1];
           SPATIAL_BUF_CNT = 0;
         }
       }        
     }
   }
+    SPATIAL_ID_42_Parsing();
+    SPATIAL_ID_39_Parsing();
+    SPATIAL_ID_35_Parsing();
+    SPATIAL_ID_32_Parsing();
+    SPATIAL_ID_22_Parsing();
+    SPATIAL_ID_30_Parsing();
+}
+
+void SPATIAL::SPATIAL_ID_22_Parsing(){
+    if(SPATIAL_ID_22_PARSING_BOOL == true){
+        SPATIAL_ID_22_PARSING_BOOL = false;
+        if(CRC_INPUT == calculate_crc16(SPATIAL_22_BUF,14)){
+            UNIX_TIME = (uint32_t)SPATIAL_22_BUF[0] + (uint32_t)SPATIAL_22_BUF[13]*100 + (uint32_t)SPATIAL_22_BUF[12]*10000 + (uint32_t)SPATIAL_22_BUF[11]*1000000;        
+        }   
+    }    
+}
+void SPATIAL::SPATIAL_ID_30_Parsing(){
+    if(SPATIAL_ID_30_PARSING_BOOL == true){
+        SPATIAL_ID_30_PARSING_BOOL = false;
+//            SATELLITE_NUM ++;
+//        if(CRC_INPUT == calculate_crc16(SPATIAL_30_BUF,13)){
+            SATELLITE_NUM = SPATIAL_30_BUF[8] + SPATIAL_30_BUF[9] + SPATIAL_30_BUF[10] + SPATIAL_30_BUF[11] + SPATIAL_30_BUF[12];
+//        }   
+    }    
 }
 
-void SPATIAL::SPATIAL_ID_20_Parsing()      //데이터 파싱을 실시함 여기서.
-{    
-    if(SPATIAL_ID_20_PARSING_BOOL == true)
-    {
-        SPATIAL_ID_20_PARSING_BOOL = false;
+void SPATIAL::SPATIAL_ID_35_Parsing(){
+    if(SPATIAL_ID_35_PARSING_BOOL == true){
+        SPATIAL_ID_35_PARSING_BOOL = false;
+        if(CRC_INPUT == calculate_crc16(SPATIAL_35_BUF,12)){
+            FP4 SPATIAL_Vx;       //이 과정은 4바이트로 흩어진 것을 하나의 float형으로 모으는 과정..
+            SPATIAL_Vx.DATA4[0] = SPATIAL_35_BUF[0];
+            SPATIAL_Vx.DATA4[1] = SPATIAL_35_BUF[1];
+            SPATIAL_Vx.DATA4[2] = SPATIAL_35_BUF[2];
+            SPATIAL_Vx.DATA4[3] = SPATIAL_35_BUF[3];
+            Vx = SPATIAL_Vx.F4;    //float형으로 짠. 단위는 역시 rad라서 고침.            
+            
+            FP4 SPATIAL_Vy;       //이 과정은 4바이트로 흩어진 것을 하나의 float형으로 모으는 과정..
+            SPATIAL_Vy.DATA4[0] = SPATIAL_35_BUF[4];
+            SPATIAL_Vy.DATA4[1] = SPATIAL_35_BUF[5];
+            SPATIAL_Vy.DATA4[2] = SPATIAL_35_BUF[6];
+            SPATIAL_Vy.DATA4[3] = SPATIAL_35_BUF[7];
+            Vy = SPATIAL_Vy.F4;    //float형으로 짠. 단위는 역시 rad라서 고침.            
+    
+            FP4 SPATIAL_Vz;       //이 과정은 4바이트로 흩어진 것을 하나의 float형으로 모으는 과정..
+            SPATIAL_Vz.DATA4[0] = SPATIAL_35_BUF[8];
+            SPATIAL_Vz.DATA4[1] = SPATIAL_35_BUF[9];
+            SPATIAL_Vz.DATA4[2] = SPATIAL_35_BUF[10];
+            SPATIAL_Vz.DATA4[3] = SPATIAL_35_BUF[11];
+            Vz = SPATIAL_Vz.F4;    //float형으로 짠. 단위는 역시 rad라서 고침.                
+            Vz = -Vz;
+            
+            /* ********** ECEF to Compass Heading ***** */
+            float B_Vx,B_Vy;
+            B_Vx = Vx * cos(YAW*M_PI/180.0) + Vy * sin(YAW*M_PI/180.0);
+            B_Vy = Vy * cos(YAW*M_PI/180.0) - Vx * sin(YAW*M_PI/180.0);
+            Vx = B_Vx;
+            Vy = B_Vy;
+        }   
+    }    
+}
+void SPATIAL::SPATIAL_ID_32_Parsing(){
+    if(SPATIAL_ID_32_PARSING_BOOL == true){
+        SPATIAL_ID_32_PARSING_BOOL = false;
+        if(CRC_INPUT == calculate_crc16(SPATIAL_32_BUF,24)){
+            HZ_CNT++;
+/*
+            SYSTEM_STATUS = (uint16_t)SPATIAL_32_BUF[0]<<8 | (uint16_t)SPATIAL_32_BUF[1];
+            FILTER_STATUS = (uint16_t)SPATIAL_32_BUF[2]<<8 | (uint16_t)SPATIAL_32_BUF[3];
 
-        if(CRC_INPUT == calculate_crc16(SPATIAL_BUF,100)){
-            UNIX_TIME = (uint32_t)SPATIAL_BUF[3]<<24 | (uint32_t)SPATIAL_BUF[4]<<16 | (uint32_t)SPATIAL_BUF[5]<<8 | (uint32_t)SPATIAL_BUF[6];        
-            UNIX_TIME /= 1000;
+            UNIX_TIME = (uint32_t)SPATIAL_32_BUF[4]<<24 | (uint32_t)SPATIAL_32_BUF[5]<<16 | (uint32_t)SPATIAL_32_BUF[6]<<8 | (uint32_t)SPATIAL_32_BUF[7];        
+            UNIX_TIME /= 100000;
+*/
             FP8 SPATIAL_LATITUDE;       //이 과정은 8바이트로 흩어진 것을 하나의 double형으로 모으는 과정..
-            SPATIAL_LATITUDE.DATA8[0] = SPATIAL_BUF[12];
-            SPATIAL_LATITUDE.DATA8[1] = SPATIAL_BUF[13];
-            SPATIAL_LATITUDE.DATA8[2] = SPATIAL_BUF[14];
-            SPATIAL_LATITUDE.DATA8[3] = SPATIAL_BUF[15];
-            SPATIAL_LATITUDE.DATA8[4] = SPATIAL_BUF[16];
-            SPATIAL_LATITUDE.DATA8[5] = SPATIAL_BUF[17];
-            SPATIAL_LATITUDE.DATA8[6] = SPATIAL_BUF[18];
-            SPATIAL_LATITUDE.DATA8[7] = SPATIAL_BUF[19];       //얘들을 모아서.     
+            SPATIAL_LATITUDE.DATA8[0] = SPATIAL_32_BUF[0];
+            SPATIAL_LATITUDE.DATA8[1] = SPATIAL_32_BUF[1];
+            SPATIAL_LATITUDE.DATA8[2] = SPATIAL_32_BUF[2];
+            SPATIAL_LATITUDE.DATA8[3] = SPATIAL_32_BUF[3];
+            SPATIAL_LATITUDE.DATA8[4] = SPATIAL_32_BUF[4];
+            SPATIAL_LATITUDE.DATA8[5] = SPATIAL_32_BUF[5];
+            SPATIAL_LATITUDE.DATA8[6] = SPATIAL_32_BUF[6];
+            SPATIAL_LATITUDE.DATA8[7] = SPATIAL_32_BUF[7];       //얘들을 모아서.     
             LATITUDE = SPATIAL_LATITUDE.F8 * RADIANS_TO_DEGREES;    //double형으로 짠. 라디안이라 고침.
     
     
     
             FP8 SPATIAL_LONGITUDE;       //이 과정은 8바이트로 흩어진 것을 하나의 double형으로 모으는 과정..
-            SPATIAL_LONGITUDE.DATA8[0] = SPATIAL_BUF[20];
-            SPATIAL_LONGITUDE.DATA8[1] = SPATIAL_BUF[21];
-            SPATIAL_LONGITUDE.DATA8[2] = SPATIAL_BUF[22];
-            SPATIAL_LONGITUDE.DATA8[3] = SPATIAL_BUF[23];
-            SPATIAL_LONGITUDE.DATA8[4] = SPATIAL_BUF[24];
-            SPATIAL_LONGITUDE.DATA8[5] = SPATIAL_BUF[25];
-            SPATIAL_LONGITUDE.DATA8[6] = SPATIAL_BUF[26];
-            SPATIAL_LONGITUDE.DATA8[7] = SPATIAL_BUF[27];       //얘들을 모아서.     
+            SPATIAL_LONGITUDE.DATA8[0] = SPATIAL_32_BUF[8];
+            SPATIAL_LONGITUDE.DATA8[1] = SPATIAL_32_BUF[9];
+            SPATIAL_LONGITUDE.DATA8[2] = SPATIAL_32_BUF[10];
+            SPATIAL_LONGITUDE.DATA8[3] = SPATIAL_32_BUF[11];
+            SPATIAL_LONGITUDE.DATA8[4] = SPATIAL_32_BUF[12];
+            SPATIAL_LONGITUDE.DATA8[5] = SPATIAL_32_BUF[13];
+            SPATIAL_LONGITUDE.DATA8[6] = SPATIAL_32_BUF[14];
+            SPATIAL_LONGITUDE.DATA8[7] = SPATIAL_32_BUF[15];       //얘들을 모아서.     
             LONGITUDE = SPATIAL_LONGITUDE.F8 * RADIANS_TO_DEGREES;    //double형으로 짠. 라디안이라 고침.
     
     
             FP8 SPATIAL_HEIGHT;       //이 과정은 8바이트로 흩어진 것을 하나의 double형으로 모으는 과정..
-            SPATIAL_HEIGHT.DATA8[0] = SPATIAL_BUF[28];
-            SPATIAL_HEIGHT.DATA8[1] = SPATIAL_BUF[29];
-            SPATIAL_HEIGHT.DATA8[2] = SPATIAL_BUF[30];
-            SPATIAL_HEIGHT.DATA8[3] = SPATIAL_BUF[31];
-            SPATIAL_HEIGHT.DATA8[4] = SPATIAL_BUF[32];
-            SPATIAL_HEIGHT.DATA8[5] = SPATIAL_BUF[33];
-            SPATIAL_HEIGHT.DATA8[6] = SPATIAL_BUF[34];
-            SPATIAL_HEIGHT.DATA8[7] = SPATIAL_BUF[35];       //얘들을 모아서.     
-            HEIGHT = SPATIAL_HEIGHT.F8 * RADIANS_TO_DEGREES;    //double형으로 짠. 라디안이라 고침.
-    
-    
-            FP4 SPATIAL_Vn;       //이 과정은 4바이트로 흩어진 것을 하나의 float형으로 모으는 과정..
-            SPATIAL_Vn.DATA4[0] = SPATIAL_BUF[36];
-            SPATIAL_Vn.DATA4[1] = SPATIAL_BUF[37];
-            SPATIAL_Vn.DATA4[2] = SPATIAL_BUF[38];
-            SPATIAL_Vn.DATA4[3] = SPATIAL_BUF[39];
-            Vn = SPATIAL_Vn.F4;    //float형으로 짠. 단위는 역시 rad라서 고침.            
-    
-    
-            FP4 SPATIAL_Ve;       //이 과정은 4바이트로 흩어진 것을 하나의 float형으로 모으는 과정..
-            SPATIAL_Ve.DATA4[0] = SPATIAL_BUF[40];
-            SPATIAL_Ve.DATA4[1] = SPATIAL_BUF[41];
-            SPATIAL_Ve.DATA4[2] = SPATIAL_BUF[42];
-            SPATIAL_Ve.DATA4[3] = SPATIAL_BUF[43];
-            Ve = SPATIAL_Ve.F4;    //float형으로 짠. 단위는 역시 rad라서 고침.            
-    
-            FP4 SPATIAL_Vd;       //이 과정은 4바이트로 흩어진 것을 하나의 float형으로 모으는 과정..
-            SPATIAL_Vd.DATA4[0] = SPATIAL_BUF[44];
-            SPATIAL_Vd.DATA4[1] = SPATIAL_BUF[45];
-            SPATIAL_Vd.DATA4[2] = SPATIAL_BUF[46];
-            SPATIAL_Vd.DATA4[3] = SPATIAL_BUF[47];
-            Vd = SPATIAL_Vd.F4;    //float형으로 짠. 단위는 역시 rad라서 고침.            
-    
-            Vd = -Vd;
-            
+            SPATIAL_HEIGHT.DATA8[0] = SPATIAL_32_BUF[16];
+            SPATIAL_HEIGHT.DATA8[1] = SPATIAL_32_BUF[17];
+            SPATIAL_HEIGHT.DATA8[2] = SPATIAL_32_BUF[18];
+            SPATIAL_HEIGHT.DATA8[3] = SPATIAL_32_BUF[19];
+            SPATIAL_HEIGHT.DATA8[4] = SPATIAL_32_BUF[20];
+            SPATIAL_HEIGHT.DATA8[5] = SPATIAL_32_BUF[21];
+            SPATIAL_HEIGHT.DATA8[6] = SPATIAL_32_BUF[22];
+            SPATIAL_HEIGHT.DATA8[7] = SPATIAL_32_BUF[23];       //얘들을 모아서.     
+            HEIGHT = SPATIAL_HEIGHT.F8;    //double형으로 짠. 라디안이라 고침.
+             
+        }   
+    }    
+}
+void SPATIAL::SPATIAL_ID_39_Parsing(){
+    if(SPATIAL_ID_39_PARSING_BOOL == true){
+        SPATIAL_ID_39_PARSING_BOOL = false;
+        if(CRC_INPUT == calculate_crc16(SPATIAL_39_BUF,12)){
             FP4 SPATIAL_ROLL;       //이 과정은 4바이트로 흩어진 것을 하나의 float형으로 모으는 과정..
-            SPATIAL_ROLL.DATA4[0] = SPATIAL_BUF[64];
-            SPATIAL_ROLL.DATA4[1] = SPATIAL_BUF[65];
-            SPATIAL_ROLL.DATA4[2] = SPATIAL_BUF[66];
-            SPATIAL_ROLL.DATA4[3] = SPATIAL_BUF[67];
+            SPATIAL_ROLL.DATA4[0] = SPATIAL_39_BUF[0];
+            SPATIAL_ROLL.DATA4[1] = SPATIAL_39_BUF[1];
+            SPATIAL_ROLL.DATA4[2] = SPATIAL_39_BUF[2];
+            SPATIAL_ROLL.DATA4[3] = SPATIAL_39_BUF[3];
             ROLL = SPATIAL_ROLL.F4 * RADIANS_TO_DEGREES;    //float형으로 짠. 단위는 역시 rad라서 고침.            
-    
+
             FP4 SPATIAL_PITCH;       //이 과정은 4바이트로 흩어진 것을 하나의 float형으로 모으는 과정..
-            SPATIAL_PITCH.DATA4[0] = SPATIAL_BUF[68];
-            SPATIAL_PITCH.DATA4[1] = SPATIAL_BUF[69];
-            SPATIAL_PITCH.DATA4[2] = SPATIAL_BUF[70];
-            SPATIAL_PITCH.DATA4[3] = SPATIAL_BUF[71];
+            SPATIAL_PITCH.DATA4[0] = SPATIAL_39_BUF[4];
+            SPATIAL_PITCH.DATA4[1] = SPATIAL_39_BUF[5];
+            SPATIAL_PITCH.DATA4[2] = SPATIAL_39_BUF[6];
+            SPATIAL_PITCH.DATA4[3] = SPATIAL_39_BUF[7];
             PITCH = SPATIAL_PITCH.F4 * RADIANS_TO_DEGREES;    //float형으로 짠. 단위는 역시 rad라서 고침.            
     
             FP4 SPATIAL_YAW;       //이 과정은 4바이트로 흩어진 것을 하나의 float형으로 모으는 과정..
-            SPATIAL_YAW.DATA4[0] = SPATIAL_BUF[72];
-            SPATIAL_YAW.DATA4[1] = SPATIAL_BUF[73];
-            SPATIAL_YAW.DATA4[2] = SPATIAL_BUF[74];
-            SPATIAL_YAW.DATA4[3] = SPATIAL_BUF[75];
-            YAW = SPATIAL_YAW.F4 * RADIANS_TO_DEGREES;    //float형으로 짠. 단위는 역시 rad라서 고침.            
+            SPATIAL_YAW.DATA4[0] = SPATIAL_39_BUF[8];
+            SPATIAL_YAW.DATA4[1] = SPATIAL_39_BUF[9];
+            SPATIAL_YAW.DATA4[2] = SPATIAL_39_BUF[10];
+            SPATIAL_YAW.DATA4[3] = SPATIAL_39_BUF[11];
+            YAW = SPATIAL_YAW.F4 * RADIANS_TO_DEGREES;    //float형으로 짠. 단위는 역시 rad라서 고침.                        
+            YAW += DECLINATION_ANGLE;
             
-            
+            if(YAW > 360) YAW -= 360;
+            else if(YAW < 0) YAW += 360;
+        }   
+    }    
+}
+void SPATIAL::SPATIAL_ID_42_Parsing(){
+    if(SPATIAL_ID_42_PARSING_BOOL == true){
+        SPATIAL_ID_42_PARSING_BOOL = false;
+        if(CRC_INPUT == calculate_crc16(SPATIAL_42_BUF,12)){
             FP4 SPATIAL_ROLL_RATE;       //이 과정은 4바이트로 흩어진 것을 하나의 float형으로 모으는 과정..
-            SPATIAL_ROLL_RATE.DATA4[0] = SPATIAL_BUF[76];
-            SPATIAL_ROLL_RATE.DATA4[1] = SPATIAL_BUF[77];
-            SPATIAL_ROLL_RATE.DATA4[2] = SPATIAL_BUF[78];
-            SPATIAL_ROLL_RATE.DATA4[3] = SPATIAL_BUF[79];
+            SPATIAL_ROLL_RATE.DATA4[0] = SPATIAL_42_BUF[0];
+            SPATIAL_ROLL_RATE.DATA4[1] = SPATIAL_42_BUF[1];
+            SPATIAL_ROLL_RATE.DATA4[2] = SPATIAL_42_BUF[2];
+            SPATIAL_ROLL_RATE.DATA4[3] = SPATIAL_42_BUF[3];
             ROLL_RATE = SPATIAL_ROLL_RATE.F4 * RADIANS_TO_DEGREES;    //float형으로 짠. 단위는 역시 rad라서 고침.            
     
             FP4 SPATIAL_PITCH_RATE;       //이 과정은 4바이트로 흩어진 것을 하나의 float형으로 모으는 과정..
-            SPATIAL_PITCH_RATE.DATA4[0] = SPATIAL_BUF[80];
-            SPATIAL_PITCH_RATE.DATA4[1] = SPATIAL_BUF[81];
-            SPATIAL_PITCH_RATE.DATA4[2] = SPATIAL_BUF[82];
-            SPATIAL_PITCH_RATE.DATA4[3] = SPATIAL_BUF[83];
+            SPATIAL_PITCH_RATE.DATA4[0] = SPATIAL_42_BUF[4];
+            SPATIAL_PITCH_RATE.DATA4[1] = SPATIAL_42_BUF[5];
+            SPATIAL_PITCH_RATE.DATA4[2] = SPATIAL_42_BUF[6];
+            SPATIAL_PITCH_RATE.DATA4[3] = SPATIAL_42_BUF[7];
             PITCH_RATE = SPATIAL_PITCH_RATE.F4 * RADIANS_TO_DEGREES;    //float형으로 짠. 단위는 역시 rad라서 고침.            
     
             FP4 SPATIAL_YAW_RATE;       //이 과정은 4바이트로 흩어진 것을 하나의 float형으로 모으는 과정..
-            SPATIAL_YAW_RATE.DATA4[0] = SPATIAL_BUF[84];
-            SPATIAL_YAW_RATE.DATA4[1] = SPATIAL_BUF[85];
-            SPATIAL_YAW_RATE.DATA4[2] = SPATIAL_BUF[86];
-            SPATIAL_YAW_RATE.DATA4[3] = SPATIAL_BUF[87];
-            YAW_RATE = SPATIAL_YAW_RATE.F4 * RADIANS_TO_DEGREES;    //float형으로 짠. 단위는 역시 rad라서 고침.            
-         
+            SPATIAL_YAW_RATE.DATA4[0] = SPATIAL_42_BUF[8];
+            SPATIAL_YAW_RATE.DATA4[1] = SPATIAL_42_BUF[9];
+            SPATIAL_YAW_RATE.DATA4[2] = SPATIAL_42_BUF[10];
+            SPATIAL_YAW_RATE.DATA4[3] = SPATIAL_42_BUF[11];
+            YAW_RATE = SPATIAL_YAW_RATE.F4 * RADIANS_TO_DEGREES;    //float형으로 짠. 단위는 역시 rad라서 고침.                                
+        }   
+    }    
+}
+
+bool SPATIAL::SPATIAL_STABLE(){
+    if(SENSOR_WAIT_BOOL == true){
+        if( (Vx<0.1) & (Vx>-0.1) & (Vy<0.1) & (Vy>-0.1) & (Vz<0.1) & (Vz>-0.1) ){
+            SENSOR_STABLE_CNT ++;
+        }else{
+            SENSOR_STABLE_CNT = 0;
         }
-
-
+        if( SENSOR_STABLE_CNT > 2000){
+            SENSOR_STABLE_OK = true;
+        }
+        ///////////////////////////////
+        if(SENSOR_STABLE_OK == true){
+            return true;
+        }else{
+            return false;
+        }        
+    }else{
+        return true;   
     }
 }