Diff: SPATIAL/SPATIAL.cpp

Revision:

0:3473b92e991e

Child:

1:9530746906b6

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SPATIAL/SPATIAL.cpp	Tue Jun 12 01:05:50 2018 +0000
@@ -0,0 +1,180 @@
+#include "SPATIAL.h"
+#include "mbed.h"
+#define M_PI 3.141592
+#define RADIANS_TO_DEGREES (180.0/M_PI)
+#include "BufferedSerial.h"
+
+
+BufferedSerial SPATIAL_UART(p28, p27);      // tx, rx
+
+//float 이랑 double 값뽑을때 쓰는거
+union FP8
+{
+    char DATA8[8];
+    double F8;
+};
+union FP4
+{
+    char DATA4[4];
+    float F4;
+};
+
+uint16_t calculate_crc16(const void *data, uint16_t length)
+{
+    uint8_t *bytes = (uint8_t *) data;
+    uint16_t crc = 0xFFFF, i;
+    for (i = 0; i < length; i++)
+    {
+        crc = (uint16_t)((crc << 8) ^ crc16_table[(crc >> 8) ^ bytes[i]]);
+    }
+    return crc;
+}
+
+void SPATIAL::Init(){
+    SPATIAL_UART.baud(115200);   
+}
+//이것은 SPATIAL에서 전송되는 각종 데이터를 받는 부분이다.
+void SPATIAL::SPATIAL_RECEIVE()
+{
+  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;}
+      SPATIAL_BUF_CNT ++;
+    }
+    else{
+      if((SPATIAL_ID[3] == 20) & (SPATIAL_ID[2] == 100))    //두번째 데이터가 '패킷넘버', 세번째 데이터가 '데이터랭쓰'임... 별도의 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;
+          CRC_INPUT = (uint16_t)SPATIAL_ID[0]<<8 | (uint16_t)SPATIAL_ID[1];
+          SPATIAL_BUF_CNT = 0;
+        }
+      }        
+    }
+  }
+}
+
+void SPATIAL::SPATIAL_ID_20_Parsing()      //데이터 파싱을 실시함 여기서.
+{    
+    if(SPATIAL_ID_20_PARSING_BOOL == true)
+    {
+        SPATIAL_ID_20_PARSING_BOOL = false;
+
+        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;
+            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];       //얘들을 모아서.     
+            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];       //얘들을 모아서.     
+            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;
+            
+            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];
+            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];
+            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라서 고침.            
+            
+            
+            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];
+            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];
+            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라서 고침.            
+         
+        }
+
+
+    }
+}
+