Kim Youngsik / Mbed 2 deprecated 0ROBOFRIEN_FCC_v1_12

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed BufferedSerial ConfigFile

SPATIAL/SPATIAL.cpp@1:9530746906b6, 2018-11-28 (annotated)

Committer:
skyyoungsik
Date:
Wed Nov 28 13:06:23 2018 +0000
Revision:
1:9530746906b6
Parent:
0:3473b92e991e 
test1

Who changed what in which revision?

UserRevisionLine numberNew contents of line
skyyoungsik 0:3473b92e991e 1 #include "SPATIAL.h"
skyyoungsik 0:3473b92e991e 2 #include "mbed.h"
skyyoungsik 0:3473b92e991e 3 #define M_PI 3.141592
skyyoungsik 0:3473b92e991e 4 #define RADIANS_TO_DEGREES (180.0/M_PI)
skyyoungsik 0:3473b92e991e 5 #include "BufferedSerial.h"
skyyoungsik 0:3473b92e991e 6
skyyoungsik 0:3473b92e991e 7
skyyoungsik 0:3473b92e991e 8 BufferedSerial SPATIAL_UART(p28, p27); // tx, rx
skyyoungsik 0:3473b92e991e 9
skyyoungsik 1:9530746906b6 10 /* ****** 100Hz Receive ********** */
skyyoungsik 1:9530746906b6 11 // Packet ID : 42, Length : 12 --> Angural Velocity X,Y,Z (rad/s)
skyyoungsik 1:9530746906b6 12 // Packet ID : 39, Length : 12 --> Roll, Pitch, Yaw (rad)
skyyoungsik 1:9530746906b6 13
skyyoungsik 1:9530746906b6 14 /* ****** 10Hz Receive *********** */
skyyoungsik 1:9530746906b6 15 // Packet ID : 35, Length = 12 --> NED Velocity X, Y, Z (m/s)
skyyoungsik 1:9530746906b6 16 // Packet ID : 32, Length = 24 --> System State Packet
skyyoungsik 1:9530746906b6 17
skyyoungsik 1:9530746906b6 18 /* ****** 1Hz Receive *********** */
skyyoungsik 1:9530746906b6 19 // Packet ID : 22, Length = 14 --> Time Packet
skyyoungsik 1:9530746906b6 20 // Packet ID : 30, Length = 13 --> Satellites Packet
skyyoungsik 1:9530746906b6 21
skyyoungsik 0:3473b92e991e 22 //float 이랑 double 값뽑을때 쓰는거
skyyoungsik 0:3473b92e991e 23 union FP8
skyyoungsik 0:3473b92e991e 24 {
skyyoungsik 0:3473b92e991e 25 char DATA8[8];
skyyoungsik 0:3473b92e991e 26 double F8;
skyyoungsik 0:3473b92e991e 27 };
skyyoungsik 0:3473b92e991e 28 union FP4
skyyoungsik 0:3473b92e991e 29 {
skyyoungsik 0:3473b92e991e 30 char DATA4[4];
skyyoungsik 0:3473b92e991e 31 float F4;
skyyoungsik 0:3473b92e991e 32 };
skyyoungsik 0:3473b92e991e 33
skyyoungsik 0:3473b92e991e 34 uint16_t calculate_crc16(const void *data, uint16_t length)
skyyoungsik 0:3473b92e991e 35 {
skyyoungsik 0:3473b92e991e 36 uint8_t *bytes = (uint8_t *) data;
skyyoungsik 0:3473b92e991e 37 uint16_t crc = 0xFFFF, i;
skyyoungsik 0:3473b92e991e 38 for (i = 0; i < length; i++)
skyyoungsik 0:3473b92e991e 39 {
skyyoungsik 0:3473b92e991e 40 crc = (uint16_t)((crc << 8) ^ crc16_table[(crc >> 8) ^ bytes[i]]);
skyyoungsik 0:3473b92e991e 41 }
skyyoungsik 0:3473b92e991e 42 return crc;
skyyoungsik 0:3473b92e991e 43 }
skyyoungsik 0:3473b92e991e 44
skyyoungsik 1:9530746906b6 45 void SPATIAL::Init(int sensor_wait_input){
skyyoungsik 1:9530746906b6 46 if( sensor_wait_input == 1){
skyyoungsik 1:9530746906b6 47 SENSOR_WAIT_BOOL = true;
skyyoungsik 1:9530746906b6 48 }else{
skyyoungsik 1:9530746906b6 49 SENSOR_WAIT_BOOL = false;
skyyoungsik 1:9530746906b6 50 }
skyyoungsik 0:3473b92e991e 51 SPATIAL_UART.baud(115200);
skyyoungsik 0:3473b92e991e 52 }
skyyoungsik 0:3473b92e991e 53 //이것은 SPATIAL에서 전송되는 각종 데이터를 받는 부분이다.
skyyoungsik 0:3473b92e991e 54 void SPATIAL::SPATIAL_RECEIVE()
skyyoungsik 0:3473b92e991e 55 {
skyyoungsik 0:3473b92e991e 56 while(SPATIAL_UART.readable() > 0)
skyyoungsik 0:3473b92e991e 57 {
skyyoungsik 0:3473b92e991e 58 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();
skyyoungsik 1:9530746906b6 59 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)){
skyyoungsik 1:9530746906b6 60 // SPATIAL_BUF[SPATIAL_BUF_CNT] = SPATIAL_ID[0];
skyyoungsik 1:9530746906b6 61 if(SPATIAL_ID_39_BOOL == true) {
skyyoungsik 1:9530746906b6 62 SPATIAL_39_BUF[SPATIAL_BUF_CNT] = SPATIAL_ID[0];
skyyoungsik 1:9530746906b6 63 if(SPATIAL_BUF_CNT >= 12){ SPATIAL_ID_39_BOOL = false; SPATIAL_ID_39_PARSING_BOOL = true;}
skyyoungsik 1:9530746906b6 64 }else if(SPATIAL_ID_42_BOOL == true) {
skyyoungsik 1:9530746906b6 65 SPATIAL_42_BUF[SPATIAL_BUF_CNT] = SPATIAL_ID[0];
skyyoungsik 1:9530746906b6 66 if(SPATIAL_BUF_CNT >= 12){ SPATIAL_ID_42_BOOL = false; SPATIAL_ID_42_PARSING_BOOL = true;}
skyyoungsik 1:9530746906b6 67 }else if(SPATIAL_ID_35_BOOL == true) {
skyyoungsik 1:9530746906b6 68 SPATIAL_35_BUF[SPATIAL_BUF_CNT] = SPATIAL_ID[0];
skyyoungsik 1:9530746906b6 69 if(SPATIAL_BUF_CNT >= 12){ SPATIAL_ID_35_BOOL = false; SPATIAL_ID_35_PARSING_BOOL = true;}
skyyoungsik 1:9530746906b6 70 }else if(SPATIAL_ID_32_BOOL == true) {
skyyoungsik 1:9530746906b6 71 SPATIAL_32_BUF[SPATIAL_BUF_CNT] = SPATIAL_ID[0];
skyyoungsik 1:9530746906b6 72 if(SPATIAL_BUF_CNT >= 24){ SPATIAL_ID_32_BOOL = false; SPATIAL_ID_32_PARSING_BOOL = true;}
skyyoungsik 1:9530746906b6 73 }else if(SPATIAL_ID_22_BOOL == true) {
skyyoungsik 1:9530746906b6 74 SPATIAL_22_BUF[SPATIAL_BUF_CNT] = SPATIAL_ID[0];
skyyoungsik 1:9530746906b6 75 if(SPATIAL_BUF_CNT >= 14){ SPATIAL_ID_22_BOOL = false; SPATIAL_ID_22_PARSING_BOOL = true;}
skyyoungsik 1:9530746906b6 76 }else if(SPATIAL_ID_30_BOOL == true) {
skyyoungsik 1:9530746906b6 77 SPATIAL_30_BUF[SPATIAL_BUF_CNT] = SPATIAL_ID[0];
skyyoungsik 1:9530746906b6 78 if(SPATIAL_BUF_CNT >= 13){ SPATIAL_ID_30_BOOL = false; SPATIAL_ID_30_PARSING_BOOL = true;}
skyyoungsik 1:9530746906b6 79 }
skyyoungsik 0:3473b92e991e 80 SPATIAL_BUF_CNT ++;
skyyoungsik 0:3473b92e991e 81 }
skyyoungsik 0:3473b92e991e 82 else{
skyyoungsik 1:9530746906b6 83 if((SPATIAL_ID[3] == 39) & (SPATIAL_ID[2] == 12)) //두번째 데이터가 '패킷넘버', 세번째 데이터가 '데이터랭쓰'임... 별도의 SOF가 없어서 이걸 기준으로함.
skyyoungsik 1:9530746906b6 84 {
skyyoungsik 1:9530746906b6 85 //첫 번째 데이터 : head LRC이며 계산식 돌린거랑 첫번째 데이터랑 일치하면 이 배열을 쓸거라는 의미.
skyyoungsik 1:9530746906b6 86 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])
skyyoungsik 1:9530746906b6 87 {
skyyoungsik 1:9530746906b6 88 SPATIAL_ID_39_BOOL = true;
skyyoungsik 1:9530746906b6 89 CRC_INPUT = (uint16_t)SPATIAL_ID[0]<<8 | (uint16_t)SPATIAL_ID[1];
skyyoungsik 1:9530746906b6 90 SPATIAL_BUF_CNT = 0;
skyyoungsik 1:9530746906b6 91 }
skyyoungsik 1:9530746906b6 92 }
skyyoungsik 1:9530746906b6 93 else if((SPATIAL_ID[3] == 42) & (SPATIAL_ID[2] == 12)) //두번째 데이터가 '패킷넘버', 세번째 데이터가 '데이터랭쓰'임... 별도의 SOF가 없어서 이걸 기준으로함.
skyyoungsik 0:3473b92e991e 94 {
skyyoungsik 0:3473b92e991e 95 //첫 번째 데이터 : head LRC이며 계산식 돌린거랑 첫번째 데이터랑 일치하면 이 배열을 쓸거라는 의미.
skyyoungsik 0:3473b92e991e 96 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])
skyyoungsik 0:3473b92e991e 97 {
skyyoungsik 1:9530746906b6 98 SPATIAL_ID_42_BOOL = true;
skyyoungsik 1:9530746906b6 99 CRC_INPUT = (uint16_t)SPATIAL_ID[0]<<8 | (uint16_t)SPATIAL_ID[1];
skyyoungsik 1:9530746906b6 100 SPATIAL_BUF_CNT = 0;
skyyoungsik 1:9530746906b6 101 }
skyyoungsik 1:9530746906b6 102 }
skyyoungsik 1:9530746906b6 103 else if((SPATIAL_ID[3] == 35) & (SPATIAL_ID[2] == 12)) //두번째 데이터가 '패킷넘버', 세번째 데이터가 '데이터랭쓰'임... 별도의 SOF가 없어서 이걸 기준으로함.
skyyoungsik 1:9530746906b6 104 {
skyyoungsik 1:9530746906b6 105 //첫 번째 데이터 : head LRC이며 계산식 돌린거랑 첫번째 데이터랑 일치하면 이 배열을 쓸거라는 의미.
skyyoungsik 1:9530746906b6 106 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])
skyyoungsik 1:9530746906b6 107 {
skyyoungsik 1:9530746906b6 108 SPATIAL_ID_35_BOOL = true;
skyyoungsik 1:9530746906b6 109 CRC_INPUT = (uint16_t)SPATIAL_ID[0]<<8 | (uint16_t)SPATIAL_ID[1];
skyyoungsik 1:9530746906b6 110 SPATIAL_BUF_CNT = 0;
skyyoungsik 1:9530746906b6 111 }
skyyoungsik 1:9530746906b6 112 }
skyyoungsik 1:9530746906b6 113 else if((SPATIAL_ID[3] == 32) & (SPATIAL_ID[2] == 24)) //두번째 데이터가 '패킷넘버', 세번째 데이터가 '데이터랭쓰'임... 별도의 SOF가 없어서 이걸 기준으로함.
skyyoungsik 1:9530746906b6 114 {
skyyoungsik 1:9530746906b6 115 //첫 번째 데이터 : head LRC이며 계산식 돌린거랑 첫번째 데이터랑 일치하면 이 배열을 쓸거라는 의미.
skyyoungsik 1:9530746906b6 116 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])
skyyoungsik 1:9530746906b6 117 {
skyyoungsik 1:9530746906b6 118 SPATIAL_ID_32_BOOL = true;
skyyoungsik 1:9530746906b6 119 CRC_INPUT = (uint16_t)SPATIAL_ID[0]<<8 | (uint16_t)SPATIAL_ID[1];
skyyoungsik 1:9530746906b6 120 SPATIAL_BUF_CNT = 0;
skyyoungsik 1:9530746906b6 121 }
skyyoungsik 1:9530746906b6 122 }
skyyoungsik 1:9530746906b6 123 else if((SPATIAL_ID[3] == 22) & (SPATIAL_ID[2] == 14)) //두번째 데이터가 '패킷넘버', 세번째 데이터가 '데이터랭쓰'임... 별도의 SOF가 없어서 이걸 기준으로함.
skyyoungsik 1:9530746906b6 124 {
skyyoungsik 1:9530746906b6 125 //첫 번째 데이터 : head LRC이며 계산식 돌린거랑 첫번째 데이터랑 일치하면 이 배열을 쓸거라는 의미.
skyyoungsik 1:9530746906b6 126 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])
skyyoungsik 1:9530746906b6 127 {
skyyoungsik 1:9530746906b6 128 SPATIAL_ID_22_BOOL = true;
skyyoungsik 1:9530746906b6 129 CRC_INPUT = (uint16_t)SPATIAL_ID[0]<<8 | (uint16_t)SPATIAL_ID[1];
skyyoungsik 1:9530746906b6 130 SPATIAL_BUF_CNT = 0;
skyyoungsik 1:9530746906b6 131 }
skyyoungsik 1:9530746906b6 132 }
skyyoungsik 1:9530746906b6 133 else if((SPATIAL_ID[3] == 30) & (SPATIAL_ID[2] == 13)) //두번째 데이터가 '패킷넘버', 세번째 데이터가 '데이터랭쓰'임... 별도의 SOF가 없어서 이걸 기준으로함.
skyyoungsik 1:9530746906b6 134 {
skyyoungsik 1:9530746906b6 135 //첫 번째 데이터 : head LRC이며 계산식 돌린거랑 첫번째 데이터랑 일치하면 이 배열을 쓸거라는 의미.
skyyoungsik 1:9530746906b6 136 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])
skyyoungsik 1:9530746906b6 137 {
skyyoungsik 1:9530746906b6 138 SPATIAL_ID_30_BOOL = true;
skyyoungsik 0:3473b92e991e 139 CRC_INPUT = (uint16_t)SPATIAL_ID[0]<<8 | (uint16_t)SPATIAL_ID[1];
skyyoungsik 0:3473b92e991e 140 SPATIAL_BUF_CNT = 0;
skyyoungsik 0:3473b92e991e 141 }
skyyoungsik 0:3473b92e991e 142 }
skyyoungsik 0:3473b92e991e 143 }
skyyoungsik 0:3473b92e991e 144 }
skyyoungsik 1:9530746906b6 145 SPATIAL_ID_42_Parsing();
skyyoungsik 1:9530746906b6 146 SPATIAL_ID_39_Parsing();
skyyoungsik 1:9530746906b6 147 SPATIAL_ID_35_Parsing();
skyyoungsik 1:9530746906b6 148 SPATIAL_ID_32_Parsing();
skyyoungsik 1:9530746906b6 149 SPATIAL_ID_22_Parsing();
skyyoungsik 1:9530746906b6 150 SPATIAL_ID_30_Parsing();
skyyoungsik 1:9530746906b6 151 }
skyyoungsik 1:9530746906b6 152
skyyoungsik 1:9530746906b6 153 void SPATIAL::SPATIAL_ID_22_Parsing(){
skyyoungsik 1:9530746906b6 154 if(SPATIAL_ID_22_PARSING_BOOL == true){
skyyoungsik 1:9530746906b6 155 SPATIAL_ID_22_PARSING_BOOL = false;
skyyoungsik 1:9530746906b6 156 if(CRC_INPUT == calculate_crc16(SPATIAL_22_BUF,14)){
skyyoungsik 1:9530746906b6 157 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;
skyyoungsik 1:9530746906b6 158 }
skyyoungsik 1:9530746906b6 159 }
skyyoungsik 1:9530746906b6 160 }
skyyoungsik 1:9530746906b6 161 void SPATIAL::SPATIAL_ID_30_Parsing(){
skyyoungsik 1:9530746906b6 162 if(SPATIAL_ID_30_PARSING_BOOL == true){
skyyoungsik 1:9530746906b6 163 SPATIAL_ID_30_PARSING_BOOL = false;
skyyoungsik 1:9530746906b6 164 // SATELLITE_NUM ++;
skyyoungsik 1:9530746906b6 165 // if(CRC_INPUT == calculate_crc16(SPATIAL_30_BUF,13)){
skyyoungsik 1:9530746906b6 166 SATELLITE_NUM = SPATIAL_30_BUF[8] + SPATIAL_30_BUF[9] + SPATIAL_30_BUF[10] + SPATIAL_30_BUF[11] + SPATIAL_30_BUF[12];
skyyoungsik 1:9530746906b6 167 // }
skyyoungsik 1:9530746906b6 168 }
skyyoungsik 0:3473b92e991e 169 }
skyyoungsik 0:3473b92e991e 170
skyyoungsik 1:9530746906b6 171 void SPATIAL::SPATIAL_ID_35_Parsing(){
skyyoungsik 1:9530746906b6 172 if(SPATIAL_ID_35_PARSING_BOOL == true){
skyyoungsik 1:9530746906b6 173 SPATIAL_ID_35_PARSING_BOOL = false;
skyyoungsik 1:9530746906b6 174 if(CRC_INPUT == calculate_crc16(SPATIAL_35_BUF,12)){
skyyoungsik 1:9530746906b6 175 FP4 SPATIAL_Vx; //이 과정은 4바이트로 흩어진 것을 하나의 float형으로 모으는 과정..
skyyoungsik 1:9530746906b6 176 SPATIAL_Vx.DATA4[0] = SPATIAL_35_BUF[0];
skyyoungsik 1:9530746906b6 177 SPATIAL_Vx.DATA4[1] = SPATIAL_35_BUF[1];
skyyoungsik 1:9530746906b6 178 SPATIAL_Vx.DATA4[2] = SPATIAL_35_BUF[2];
skyyoungsik 1:9530746906b6 179 SPATIAL_Vx.DATA4[3] = SPATIAL_35_BUF[3];
skyyoungsik 1:9530746906b6 180 Vx = SPATIAL_Vx.F4; //float형으로 짠. 단위는 역시 rad라서 고침.
skyyoungsik 1:9530746906b6 181
skyyoungsik 1:9530746906b6 182 FP4 SPATIAL_Vy; //이 과정은 4바이트로 흩어진 것을 하나의 float형으로 모으는 과정..
skyyoungsik 1:9530746906b6 183 SPATIAL_Vy.DATA4[0] = SPATIAL_35_BUF[4];
skyyoungsik 1:9530746906b6 184 SPATIAL_Vy.DATA4[1] = SPATIAL_35_BUF[5];
skyyoungsik 1:9530746906b6 185 SPATIAL_Vy.DATA4[2] = SPATIAL_35_BUF[6];
skyyoungsik 1:9530746906b6 186 SPATIAL_Vy.DATA4[3] = SPATIAL_35_BUF[7];
skyyoungsik 1:9530746906b6 187 Vy = SPATIAL_Vy.F4; //float형으로 짠. 단위는 역시 rad라서 고침.
skyyoungsik 1:9530746906b6 188
skyyoungsik 1:9530746906b6 189 FP4 SPATIAL_Vz; //이 과정은 4바이트로 흩어진 것을 하나의 float형으로 모으는 과정..
skyyoungsik 1:9530746906b6 190 SPATIAL_Vz.DATA4[0] = SPATIAL_35_BUF[8];
skyyoungsik 1:9530746906b6 191 SPATIAL_Vz.DATA4[1] = SPATIAL_35_BUF[9];
skyyoungsik 1:9530746906b6 192 SPATIAL_Vz.DATA4[2] = SPATIAL_35_BUF[10];
skyyoungsik 1:9530746906b6 193 SPATIAL_Vz.DATA4[3] = SPATIAL_35_BUF[11];
skyyoungsik 1:9530746906b6 194 Vz = SPATIAL_Vz.F4; //float형으로 짠. 단위는 역시 rad라서 고침.
skyyoungsik 1:9530746906b6 195 Vz = -Vz;
skyyoungsik 1:9530746906b6 196
skyyoungsik 1:9530746906b6 197 /* ********** ECEF to Compass Heading ***** */
skyyoungsik 1:9530746906b6 198 float B_Vx,B_Vy;
skyyoungsik 1:9530746906b6 199 B_Vx = Vx * cos(YAW*M_PI/180.0) + Vy * sin(YAW*M_PI/180.0);
skyyoungsik 1:9530746906b6 200 B_Vy = Vy * cos(YAW*M_PI/180.0) - Vx * sin(YAW*M_PI/180.0);
skyyoungsik 1:9530746906b6 201 Vx = B_Vx;
skyyoungsik 1:9530746906b6 202 Vy = B_Vy;
skyyoungsik 1:9530746906b6 203 }
skyyoungsik 1:9530746906b6 204 }
skyyoungsik 1:9530746906b6 205 }
skyyoungsik 1:9530746906b6 206 void SPATIAL::SPATIAL_ID_32_Parsing(){
skyyoungsik 1:9530746906b6 207 if(SPATIAL_ID_32_PARSING_BOOL == true){
skyyoungsik 1:9530746906b6 208 SPATIAL_ID_32_PARSING_BOOL = false;
skyyoungsik 1:9530746906b6 209 if(CRC_INPUT == calculate_crc16(SPATIAL_32_BUF,24)){
skyyoungsik 1:9530746906b6 210 HZ_CNT++;
skyyoungsik 1:9530746906b6 211 /*
skyyoungsik 1:9530746906b6 212 SYSTEM_STATUS = (uint16_t)SPATIAL_32_BUF[0]<<8 | (uint16_t)SPATIAL_32_BUF[1];
skyyoungsik 1:9530746906b6 213 FILTER_STATUS = (uint16_t)SPATIAL_32_BUF[2]<<8 | (uint16_t)SPATIAL_32_BUF[3];
skyyoungsik 0:3473b92e991e 214
skyyoungsik 1:9530746906b6 215 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];
skyyoungsik 1:9530746906b6 216 UNIX_TIME /= 100000;
skyyoungsik 1:9530746906b6 217 */
skyyoungsik 0:3473b92e991e 218 FP8 SPATIAL_LATITUDE; //이 과정은 8바이트로 흩어진 것을 하나의 double형으로 모으는 과정..
skyyoungsik 1:9530746906b6 219 SPATIAL_LATITUDE.DATA8[0] = SPATIAL_32_BUF[0];
skyyoungsik 1:9530746906b6 220 SPATIAL_LATITUDE.DATA8[1] = SPATIAL_32_BUF[1];
skyyoungsik 1:9530746906b6 221 SPATIAL_LATITUDE.DATA8[2] = SPATIAL_32_BUF[2];
skyyoungsik 1:9530746906b6 222 SPATIAL_LATITUDE.DATA8[3] = SPATIAL_32_BUF[3];
skyyoungsik 1:9530746906b6 223 SPATIAL_LATITUDE.DATA8[4] = SPATIAL_32_BUF[4];
skyyoungsik 1:9530746906b6 224 SPATIAL_LATITUDE.DATA8[5] = SPATIAL_32_BUF[5];
skyyoungsik 1:9530746906b6 225 SPATIAL_LATITUDE.DATA8[6] = SPATIAL_32_BUF[6];
skyyoungsik 1:9530746906b6 226 SPATIAL_LATITUDE.DATA8[7] = SPATIAL_32_BUF[7]; //얘들을 모아서.
skyyoungsik 0:3473b92e991e 227 LATITUDE = SPATIAL_LATITUDE.F8 * RADIANS_TO_DEGREES; //double형으로 짠. 라디안이라 고침.
skyyoungsik 0:3473b92e991e 228
skyyoungsik 0:3473b92e991e 229
skyyoungsik 0:3473b92e991e 230
skyyoungsik 0:3473b92e991e 231 FP8 SPATIAL_LONGITUDE; //이 과정은 8바이트로 흩어진 것을 하나의 double형으로 모으는 과정..
skyyoungsik 1:9530746906b6 232 SPATIAL_LONGITUDE.DATA8[0] = SPATIAL_32_BUF[8];
skyyoungsik 1:9530746906b6 233 SPATIAL_LONGITUDE.DATA8[1] = SPATIAL_32_BUF[9];
skyyoungsik 1:9530746906b6 234 SPATIAL_LONGITUDE.DATA8[2] = SPATIAL_32_BUF[10];
skyyoungsik 1:9530746906b6 235 SPATIAL_LONGITUDE.DATA8[3] = SPATIAL_32_BUF[11];
skyyoungsik 1:9530746906b6 236 SPATIAL_LONGITUDE.DATA8[4] = SPATIAL_32_BUF[12];
skyyoungsik 1:9530746906b6 237 SPATIAL_LONGITUDE.DATA8[5] = SPATIAL_32_BUF[13];
skyyoungsik 1:9530746906b6 238 SPATIAL_LONGITUDE.DATA8[6] = SPATIAL_32_BUF[14];
skyyoungsik 1:9530746906b6 239 SPATIAL_LONGITUDE.DATA8[7] = SPATIAL_32_BUF[15]; //얘들을 모아서.
skyyoungsik 0:3473b92e991e 240 LONGITUDE = SPATIAL_LONGITUDE.F8 * RADIANS_TO_DEGREES; //double형으로 짠. 라디안이라 고침.
skyyoungsik 0:3473b92e991e 241
skyyoungsik 0:3473b92e991e 242
skyyoungsik 0:3473b92e991e 243 FP8 SPATIAL_HEIGHT; //이 과정은 8바이트로 흩어진 것을 하나의 double형으로 모으는 과정..
skyyoungsik 1:9530746906b6 244 SPATIAL_HEIGHT.DATA8[0] = SPATIAL_32_BUF[16];
skyyoungsik 1:9530746906b6 245 SPATIAL_HEIGHT.DATA8[1] = SPATIAL_32_BUF[17];
skyyoungsik 1:9530746906b6 246 SPATIAL_HEIGHT.DATA8[2] = SPATIAL_32_BUF[18];
skyyoungsik 1:9530746906b6 247 SPATIAL_HEIGHT.DATA8[3] = SPATIAL_32_BUF[19];
skyyoungsik 1:9530746906b6 248 SPATIAL_HEIGHT.DATA8[4] = SPATIAL_32_BUF[20];
skyyoungsik 1:9530746906b6 249 SPATIAL_HEIGHT.DATA8[5] = SPATIAL_32_BUF[21];
skyyoungsik 1:9530746906b6 250 SPATIAL_HEIGHT.DATA8[6] = SPATIAL_32_BUF[22];
skyyoungsik 1:9530746906b6 251 SPATIAL_HEIGHT.DATA8[7] = SPATIAL_32_BUF[23]; //얘들을 모아서.
skyyoungsik 1:9530746906b6 252 HEIGHT = SPATIAL_HEIGHT.F8; //double형으로 짠. 라디안이라 고침.
skyyoungsik 1:9530746906b6 253
skyyoungsik 1:9530746906b6 254 }
skyyoungsik 1:9530746906b6 255 }
skyyoungsik 1:9530746906b6 256 }
skyyoungsik 1:9530746906b6 257 void SPATIAL::SPATIAL_ID_39_Parsing(){
skyyoungsik 1:9530746906b6 258 if(SPATIAL_ID_39_PARSING_BOOL == true){
skyyoungsik 1:9530746906b6 259 SPATIAL_ID_39_PARSING_BOOL = false;
skyyoungsik 1:9530746906b6 260 if(CRC_INPUT == calculate_crc16(SPATIAL_39_BUF,12)){
skyyoungsik 0:3473b92e991e 261 FP4 SPATIAL_ROLL; //이 과정은 4바이트로 흩어진 것을 하나의 float형으로 모으는 과정..
skyyoungsik 1:9530746906b6 262 SPATIAL_ROLL.DATA4[0] = SPATIAL_39_BUF[0];
skyyoungsik 1:9530746906b6 263 SPATIAL_ROLL.DATA4[1] = SPATIAL_39_BUF[1];
skyyoungsik 1:9530746906b6 264 SPATIAL_ROLL.DATA4[2] = SPATIAL_39_BUF[2];
skyyoungsik 1:9530746906b6 265 SPATIAL_ROLL.DATA4[3] = SPATIAL_39_BUF[3];
skyyoungsik 0:3473b92e991e 266 ROLL = SPATIAL_ROLL.F4 * RADIANS_TO_DEGREES; //float형으로 짠. 단위는 역시 rad라서 고침.
skyyoungsik 1:9530746906b6 267
skyyoungsik 0:3473b92e991e 268 FP4 SPATIAL_PITCH; //이 과정은 4바이트로 흩어진 것을 하나의 float형으로 모으는 과정..
skyyoungsik 1:9530746906b6 269 SPATIAL_PITCH.DATA4[0] = SPATIAL_39_BUF[4];
skyyoungsik 1:9530746906b6 270 SPATIAL_PITCH.DATA4[1] = SPATIAL_39_BUF[5];
skyyoungsik 1:9530746906b6 271 SPATIAL_PITCH.DATA4[2] = SPATIAL_39_BUF[6];
skyyoungsik 1:9530746906b6 272 SPATIAL_PITCH.DATA4[3] = SPATIAL_39_BUF[7];
skyyoungsik 0:3473b92e991e 273 PITCH = SPATIAL_PITCH.F4 * RADIANS_TO_DEGREES; //float형으로 짠. 단위는 역시 rad라서 고침.
skyyoungsik 0:3473b92e991e 274
skyyoungsik 0:3473b92e991e 275 FP4 SPATIAL_YAW; //이 과정은 4바이트로 흩어진 것을 하나의 float형으로 모으는 과정..
skyyoungsik 1:9530746906b6 276 SPATIAL_YAW.DATA4[0] = SPATIAL_39_BUF[8];
skyyoungsik 1:9530746906b6 277 SPATIAL_YAW.DATA4[1] = SPATIAL_39_BUF[9];
skyyoungsik 1:9530746906b6 278 SPATIAL_YAW.DATA4[2] = SPATIAL_39_BUF[10];
skyyoungsik 1:9530746906b6 279 SPATIAL_YAW.DATA4[3] = SPATIAL_39_BUF[11];
skyyoungsik 1:9530746906b6 280 YAW = SPATIAL_YAW.F4 * RADIANS_TO_DEGREES; //float형으로 짠. 단위는 역시 rad라서 고침.
skyyoungsik 1:9530746906b6 281 YAW += DECLINATION_ANGLE;
skyyoungsik 0:3473b92e991e 282
skyyoungsik 1:9530746906b6 283 if(YAW > 360) YAW -= 360;
skyyoungsik 1:9530746906b6 284 else if(YAW < 0) YAW += 360;
skyyoungsik 1:9530746906b6 285 }
skyyoungsik 1:9530746906b6 286 }
skyyoungsik 1:9530746906b6 287 }
skyyoungsik 1:9530746906b6 288 void SPATIAL::SPATIAL_ID_42_Parsing(){
skyyoungsik 1:9530746906b6 289 if(SPATIAL_ID_42_PARSING_BOOL == true){
skyyoungsik 1:9530746906b6 290 SPATIAL_ID_42_PARSING_BOOL = false;
skyyoungsik 1:9530746906b6 291 if(CRC_INPUT == calculate_crc16(SPATIAL_42_BUF,12)){
skyyoungsik 0:3473b92e991e 292 FP4 SPATIAL_ROLL_RATE; //이 과정은 4바이트로 흩어진 것을 하나의 float형으로 모으는 과정..
skyyoungsik 1:9530746906b6 293 SPATIAL_ROLL_RATE.DATA4[0] = SPATIAL_42_BUF[0];
skyyoungsik 1:9530746906b6 294 SPATIAL_ROLL_RATE.DATA4[1] = SPATIAL_42_BUF[1];
skyyoungsik 1:9530746906b6 295 SPATIAL_ROLL_RATE.DATA4[2] = SPATIAL_42_BUF[2];
skyyoungsik 1:9530746906b6 296 SPATIAL_ROLL_RATE.DATA4[3] = SPATIAL_42_BUF[3];
skyyoungsik 0:3473b92e991e 297 ROLL_RATE = SPATIAL_ROLL_RATE.F4 * RADIANS_TO_DEGREES; //float형으로 짠. 단위는 역시 rad라서 고침.
skyyoungsik 0:3473b92e991e 298
skyyoungsik 0:3473b92e991e 299 FP4 SPATIAL_PITCH_RATE; //이 과정은 4바이트로 흩어진 것을 하나의 float형으로 모으는 과정..
skyyoungsik 1:9530746906b6 300 SPATIAL_PITCH_RATE.DATA4[0] = SPATIAL_42_BUF[4];
skyyoungsik 1:9530746906b6 301 SPATIAL_PITCH_RATE.DATA4[1] = SPATIAL_42_BUF[5];
skyyoungsik 1:9530746906b6 302 SPATIAL_PITCH_RATE.DATA4[2] = SPATIAL_42_BUF[6];
skyyoungsik 1:9530746906b6 303 SPATIAL_PITCH_RATE.DATA4[3] = SPATIAL_42_BUF[7];
skyyoungsik 0:3473b92e991e 304 PITCH_RATE = SPATIAL_PITCH_RATE.F4 * RADIANS_TO_DEGREES; //float형으로 짠. 단위는 역시 rad라서 고침.
skyyoungsik 0:3473b92e991e 305
skyyoungsik 0:3473b92e991e 306 FP4 SPATIAL_YAW_RATE; //이 과정은 4바이트로 흩어진 것을 하나의 float형으로 모으는 과정..
skyyoungsik 1:9530746906b6 307 SPATIAL_YAW_RATE.DATA4[0] = SPATIAL_42_BUF[8];
skyyoungsik 1:9530746906b6 308 SPATIAL_YAW_RATE.DATA4[1] = SPATIAL_42_BUF[9];
skyyoungsik 1:9530746906b6 309 SPATIAL_YAW_RATE.DATA4[2] = SPATIAL_42_BUF[10];
skyyoungsik 1:9530746906b6 310 SPATIAL_YAW_RATE.DATA4[3] = SPATIAL_42_BUF[11];
skyyoungsik 1:9530746906b6 311 YAW_RATE = SPATIAL_YAW_RATE.F4 * RADIANS_TO_DEGREES; //float형으로 짠. 단위는 역시 rad라서 고침.
skyyoungsik 1:9530746906b6 312 }
skyyoungsik 1:9530746906b6 313 }
skyyoungsik 1:9530746906b6 314 }
skyyoungsik 1:9530746906b6 315
skyyoungsik 1:9530746906b6 316 bool SPATIAL::SPATIAL_STABLE(){
skyyoungsik 1:9530746906b6 317 if(SENSOR_WAIT_BOOL == true){
skyyoungsik 1:9530746906b6 318 if( (Vx<0.1) & (Vx>-0.1) & (Vy<0.1) & (Vy>-0.1) & (Vz<0.1) & (Vz>-0.1) ){
skyyoungsik 1:9530746906b6 319 SENSOR_STABLE_CNT ++;
skyyoungsik 1:9530746906b6 320 }else{
skyyoungsik 1:9530746906b6 321 SENSOR_STABLE_CNT = 0;
skyyoungsik 0:3473b92e991e 322 }
skyyoungsik 1:9530746906b6 323 if( SENSOR_STABLE_CNT > 2000){
skyyoungsik 1:9530746906b6 324 SENSOR_STABLE_OK = true;
skyyoungsik 1:9530746906b6 325 }
skyyoungsik 1:9530746906b6 326 ///////////////////////////////
skyyoungsik 1:9530746906b6 327 if(SENSOR_STABLE_OK == true){
skyyoungsik 1:9530746906b6 328 return true;
skyyoungsik 1:9530746906b6 329 }else{
skyyoungsik 1:9530746906b6 330 return false;
skyyoungsik 1:9530746906b6 331 }
skyyoungsik 1:9530746906b6 332 }else{
skyyoungsik 1:9530746906b6 333 return true;
skyyoungsik 0:3473b92e991e 334 }
skyyoungsik 0:3473b92e991e 335 }
skyyoungsik 0:3473b92e991e 336