File content as of revision 0:3473b92e991e:

#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라서 고침.            
         
        }


    }
}