File content as of revision 1:9530746906b6:

#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

/* ****** 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
{
    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(int sensor_wait_input){
    if( sensor_wait_input == 1){
        SENSOR_WAIT_BOOL = true;
    }else{
        SENSOR_WAIT_BOOL = false;
    }
    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_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] == 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_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_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];

            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_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_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_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_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_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_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_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_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_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;   
    }
}