Kim Youngsik
/
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
Embed:
(wiki syntax)
Show/hide line numbers
SPATIAL.cpp
00001 #include "SPATIAL.h" 00002 #include "mbed.h" 00003 #define M_PI 3.141592 00004 #define RADIANS_TO_DEGREES (180.0/M_PI) 00005 #include "BufferedSerial.h" 00006 00007 00008 BufferedSerial SPATIAL_UART(p28, p27); // tx, rx 00009 00010 /* ****** 100Hz Receive ********** */ 00011 // Packet ID : 42, Length : 12 --> Angural Velocity X,Y,Z (rad/s) 00012 // Packet ID : 39, Length : 12 --> Roll, Pitch, Yaw (rad) 00013 00014 /* ****** 10Hz Receive *********** */ 00015 // Packet ID : 35, Length = 12 --> NED Velocity X, Y, Z (m/s) 00016 // Packet ID : 32, Length = 24 --> System State Packet 00017 00018 /* ****** 1Hz Receive *********** */ 00019 // Packet ID : 22, Length = 14 --> Time Packet 00020 // Packet ID : 30, Length = 13 --> Satellites Packet 00021 00022 //float 이랑 double 값뽑을때 쓰는거 00023 union FP8 00024 { 00025 char DATA8[8]; 00026 double F8; 00027 }; 00028 union FP4 00029 { 00030 char DATA4[4]; 00031 float F4; 00032 }; 00033 00034 uint16_t calculate_crc16(const void *data, uint16_t length) 00035 { 00036 uint8_t *bytes = (uint8_t *) data; 00037 uint16_t crc = 0xFFFF, i; 00038 for (i = 0; i < length; i++) 00039 { 00040 crc = (uint16_t)((crc << 8) ^ crc16_table[(crc >> 8) ^ bytes[i]]); 00041 } 00042 return crc; 00043 } 00044 00045 void SPATIAL::Init(int sensor_wait_input){ 00046 if( sensor_wait_input == 1){ 00047 SENSOR_WAIT_BOOL = true; 00048 }else{ 00049 SENSOR_WAIT_BOOL = false; 00050 } 00051 SPATIAL_UART.baud(115200); 00052 } 00053 //이것은 SPATIAL에서 전송되는 각종 데이터를 받는 부분이다. 00054 void SPATIAL::SPATIAL_RECEIVE() 00055 { 00056 while(SPATIAL_UART.readable() > 0) 00057 { 00058 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(); 00059 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)){ 00060 // SPATIAL_BUF[SPATIAL_BUF_CNT] = SPATIAL_ID[0]; 00061 if(SPATIAL_ID_39_BOOL == true) { 00062 SPATIAL_39_BUF[SPATIAL_BUF_CNT] = SPATIAL_ID[0]; 00063 if(SPATIAL_BUF_CNT >= 12){ SPATIAL_ID_39_BOOL = false; SPATIAL_ID_39_PARSING_BOOL = true;} 00064 }else if(SPATIAL_ID_42_BOOL == true) { 00065 SPATIAL_42_BUF[SPATIAL_BUF_CNT] = SPATIAL_ID[0]; 00066 if(SPATIAL_BUF_CNT >= 12){ SPATIAL_ID_42_BOOL = false; SPATIAL_ID_42_PARSING_BOOL = true;} 00067 }else if(SPATIAL_ID_35_BOOL == true) { 00068 SPATIAL_35_BUF[SPATIAL_BUF_CNT] = SPATIAL_ID[0]; 00069 if(SPATIAL_BUF_CNT >= 12){ SPATIAL_ID_35_BOOL = false; SPATIAL_ID_35_PARSING_BOOL = true;} 00070 }else if(SPATIAL_ID_32_BOOL == true) { 00071 SPATIAL_32_BUF[SPATIAL_BUF_CNT] = SPATIAL_ID[0]; 00072 if(SPATIAL_BUF_CNT >= 24){ SPATIAL_ID_32_BOOL = false; SPATIAL_ID_32_PARSING_BOOL = true;} 00073 }else if(SPATIAL_ID_22_BOOL == true) { 00074 SPATIAL_22_BUF[SPATIAL_BUF_CNT] = SPATIAL_ID[0]; 00075 if(SPATIAL_BUF_CNT >= 14){ SPATIAL_ID_22_BOOL = false; SPATIAL_ID_22_PARSING_BOOL = true;} 00076 }else if(SPATIAL_ID_30_BOOL == true) { 00077 SPATIAL_30_BUF[SPATIAL_BUF_CNT] = SPATIAL_ID[0]; 00078 if(SPATIAL_BUF_CNT >= 13){ SPATIAL_ID_30_BOOL = false; SPATIAL_ID_30_PARSING_BOOL = true;} 00079 } 00080 SPATIAL_BUF_CNT ++; 00081 } 00082 else{ 00083 if((SPATIAL_ID[3] == 39) & (SPATIAL_ID[2] == 12)) //두번째 데이터가 '패킷넘버', 세번째 데이터가 '데이터랭쓰'임... 별도의 SOF가 없어서 이걸 기준으로함. 00084 { 00085 //첫 번째 데이터 : head LRC이며 계산식 돌린거랑 첫번째 데이터랑 일치하면 이 배열을 쓸거라는 의미. 00086 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]) 00087 { 00088 SPATIAL_ID_39_BOOL = true; 00089 CRC_INPUT = (uint16_t)SPATIAL_ID[0]<<8 | (uint16_t)SPATIAL_ID[1]; 00090 SPATIAL_BUF_CNT = 0; 00091 } 00092 } 00093 else if((SPATIAL_ID[3] == 42) & (SPATIAL_ID[2] == 12)) //두번째 데이터가 '패킷넘버', 세번째 데이터가 '데이터랭쓰'임... 별도의 SOF가 없어서 이걸 기준으로함. 00094 { 00095 //첫 번째 데이터 : head LRC이며 계산식 돌린거랑 첫번째 데이터랑 일치하면 이 배열을 쓸거라는 의미. 00096 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]) 00097 { 00098 SPATIAL_ID_42_BOOL = true; 00099 CRC_INPUT = (uint16_t)SPATIAL_ID[0]<<8 | (uint16_t)SPATIAL_ID[1]; 00100 SPATIAL_BUF_CNT = 0; 00101 } 00102 } 00103 else if((SPATIAL_ID[3] == 35) & (SPATIAL_ID[2] == 12)) //두번째 데이터가 '패킷넘버', 세번째 데이터가 '데이터랭쓰'임... 별도의 SOF가 없어서 이걸 기준으로함. 00104 { 00105 //첫 번째 데이터 : head LRC이며 계산식 돌린거랑 첫번째 데이터랑 일치하면 이 배열을 쓸거라는 의미. 00106 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]) 00107 { 00108 SPATIAL_ID_35_BOOL = true; 00109 CRC_INPUT = (uint16_t)SPATIAL_ID[0]<<8 | (uint16_t)SPATIAL_ID[1]; 00110 SPATIAL_BUF_CNT = 0; 00111 } 00112 } 00113 else if((SPATIAL_ID[3] == 32) & (SPATIAL_ID[2] == 24)) //두번째 데이터가 '패킷넘버', 세번째 데이터가 '데이터랭쓰'임... 별도의 SOF가 없어서 이걸 기준으로함. 00114 { 00115 //첫 번째 데이터 : head LRC이며 계산식 돌린거랑 첫번째 데이터랑 일치하면 이 배열을 쓸거라는 의미. 00116 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]) 00117 { 00118 SPATIAL_ID_32_BOOL = true; 00119 CRC_INPUT = (uint16_t)SPATIAL_ID[0]<<8 | (uint16_t)SPATIAL_ID[1]; 00120 SPATIAL_BUF_CNT = 0; 00121 } 00122 } 00123 else if((SPATIAL_ID[3] == 22) & (SPATIAL_ID[2] == 14)) //두번째 데이터가 '패킷넘버', 세번째 데이터가 '데이터랭쓰'임... 별도의 SOF가 없어서 이걸 기준으로함. 00124 { 00125 //첫 번째 데이터 : head LRC이며 계산식 돌린거랑 첫번째 데이터랑 일치하면 이 배열을 쓸거라는 의미. 00126 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]) 00127 { 00128 SPATIAL_ID_22_BOOL = true; 00129 CRC_INPUT = (uint16_t)SPATIAL_ID[0]<<8 | (uint16_t)SPATIAL_ID[1]; 00130 SPATIAL_BUF_CNT = 0; 00131 } 00132 } 00133 else if((SPATIAL_ID[3] == 30) & (SPATIAL_ID[2] == 13)) //두번째 데이터가 '패킷넘버', 세번째 데이터가 '데이터랭쓰'임... 별도의 SOF가 없어서 이걸 기준으로함. 00134 { 00135 //첫 번째 데이터 : head LRC이며 계산식 돌린거랑 첫번째 데이터랑 일치하면 이 배열을 쓸거라는 의미. 00136 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]) 00137 { 00138 SPATIAL_ID_30_BOOL = true; 00139 CRC_INPUT = (uint16_t)SPATIAL_ID[0]<<8 | (uint16_t)SPATIAL_ID[1]; 00140 SPATIAL_BUF_CNT = 0; 00141 } 00142 } 00143 } 00144 } 00145 SPATIAL_ID_42_Parsing(); 00146 SPATIAL_ID_39_Parsing(); 00147 SPATIAL_ID_35_Parsing(); 00148 SPATIAL_ID_32_Parsing(); 00149 SPATIAL_ID_22_Parsing(); 00150 SPATIAL_ID_30_Parsing(); 00151 } 00152 00153 void SPATIAL::SPATIAL_ID_22_Parsing(){ 00154 if(SPATIAL_ID_22_PARSING_BOOL == true){ 00155 SPATIAL_ID_22_PARSING_BOOL = false; 00156 if(CRC_INPUT == calculate_crc16(SPATIAL_22_BUF,14)){ 00157 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; 00158 } 00159 } 00160 } 00161 void SPATIAL::SPATIAL_ID_30_Parsing(){ 00162 if(SPATIAL_ID_30_PARSING_BOOL == true){ 00163 SPATIAL_ID_30_PARSING_BOOL = false; 00164 // SATELLITE_NUM ++; 00165 // if(CRC_INPUT == calculate_crc16(SPATIAL_30_BUF,13)){ 00166 SATELLITE_NUM = SPATIAL_30_BUF[8] + SPATIAL_30_BUF[9] + SPATIAL_30_BUF[10] + SPATIAL_30_BUF[11] + SPATIAL_30_BUF[12]; 00167 // } 00168 } 00169 } 00170 00171 void SPATIAL::SPATIAL_ID_35_Parsing(){ 00172 if(SPATIAL_ID_35_PARSING_BOOL == true){ 00173 SPATIAL_ID_35_PARSING_BOOL = false; 00174 if(CRC_INPUT == calculate_crc16(SPATIAL_35_BUF,12)){ 00175 FP4 SPATIAL_Vx; //이 과정은 4바이트로 흩어진 것을 하나의 float형으로 모으는 과정.. 00176 SPATIAL_Vx.DATA4[0] = SPATIAL_35_BUF[0]; 00177 SPATIAL_Vx.DATA4[1] = SPATIAL_35_BUF[1]; 00178 SPATIAL_Vx.DATA4[2] = SPATIAL_35_BUF[2]; 00179 SPATIAL_Vx.DATA4[3] = SPATIAL_35_BUF[3]; 00180 Vx = SPATIAL_Vx.F4; //float형으로 짠. 단위는 역시 rad라서 고침. 00181 00182 FP4 SPATIAL_Vy; //이 과정은 4바이트로 흩어진 것을 하나의 float형으로 모으는 과정.. 00183 SPATIAL_Vy.DATA4[0] = SPATIAL_35_BUF[4]; 00184 SPATIAL_Vy.DATA4[1] = SPATIAL_35_BUF[5]; 00185 SPATIAL_Vy.DATA4[2] = SPATIAL_35_BUF[6]; 00186 SPATIAL_Vy.DATA4[3] = SPATIAL_35_BUF[7]; 00187 Vy = SPATIAL_Vy.F4; //float형으로 짠. 단위는 역시 rad라서 고침. 00188 00189 FP4 SPATIAL_Vz; //이 과정은 4바이트로 흩어진 것을 하나의 float형으로 모으는 과정.. 00190 SPATIAL_Vz.DATA4[0] = SPATIAL_35_BUF[8]; 00191 SPATIAL_Vz.DATA4[1] = SPATIAL_35_BUF[9]; 00192 SPATIAL_Vz.DATA4[2] = SPATIAL_35_BUF[10]; 00193 SPATIAL_Vz.DATA4[3] = SPATIAL_35_BUF[11]; 00194 Vz = SPATIAL_Vz.F4; //float형으로 짠. 단위는 역시 rad라서 고침. 00195 Vz = -Vz; 00196 00197 /* ********** ECEF to Compass Heading ***** */ 00198 float B_Vx,B_Vy; 00199 B_Vx = Vx * cos(YAW*M_PI/180.0) + Vy * sin(YAW*M_PI/180.0); 00200 B_Vy = Vy * cos(YAW*M_PI/180.0) - Vx * sin(YAW*M_PI/180.0); 00201 Vx = B_Vx; 00202 Vy = B_Vy; 00203 } 00204 } 00205 } 00206 void SPATIAL::SPATIAL_ID_32_Parsing(){ 00207 if(SPATIAL_ID_32_PARSING_BOOL == true){ 00208 SPATIAL_ID_32_PARSING_BOOL = false; 00209 if(CRC_INPUT == calculate_crc16(SPATIAL_32_BUF,24)){ 00210 HZ_CNT++; 00211 /* 00212 SYSTEM_STATUS = (uint16_t)SPATIAL_32_BUF[0]<<8 | (uint16_t)SPATIAL_32_BUF[1]; 00213 FILTER_STATUS = (uint16_t)SPATIAL_32_BUF[2]<<8 | (uint16_t)SPATIAL_32_BUF[3]; 00214 00215 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]; 00216 UNIX_TIME /= 100000; 00217 */ 00218 FP8 SPATIAL_LATITUDE; //이 과정은 8바이트로 흩어진 것을 하나의 double형으로 모으는 과정.. 00219 SPATIAL_LATITUDE.DATA8[0] = SPATIAL_32_BUF[0]; 00220 SPATIAL_LATITUDE.DATA8[1] = SPATIAL_32_BUF[1]; 00221 SPATIAL_LATITUDE.DATA8[2] = SPATIAL_32_BUF[2]; 00222 SPATIAL_LATITUDE.DATA8[3] = SPATIAL_32_BUF[3]; 00223 SPATIAL_LATITUDE.DATA8[4] = SPATIAL_32_BUF[4]; 00224 SPATIAL_LATITUDE.DATA8[5] = SPATIAL_32_BUF[5]; 00225 SPATIAL_LATITUDE.DATA8[6] = SPATIAL_32_BUF[6]; 00226 SPATIAL_LATITUDE.DATA8[7] = SPATIAL_32_BUF[7]; //얘들을 모아서. 00227 LATITUDE = SPATIAL_LATITUDE.F8 * RADIANS_TO_DEGREES; //double형으로 짠. 라디안이라 고침. 00228 00229 00230 00231 FP8 SPATIAL_LONGITUDE; //이 과정은 8바이트로 흩어진 것을 하나의 double형으로 모으는 과정.. 00232 SPATIAL_LONGITUDE.DATA8[0] = SPATIAL_32_BUF[8]; 00233 SPATIAL_LONGITUDE.DATA8[1] = SPATIAL_32_BUF[9]; 00234 SPATIAL_LONGITUDE.DATA8[2] = SPATIAL_32_BUF[10]; 00235 SPATIAL_LONGITUDE.DATA8[3] = SPATIAL_32_BUF[11]; 00236 SPATIAL_LONGITUDE.DATA8[4] = SPATIAL_32_BUF[12]; 00237 SPATIAL_LONGITUDE.DATA8[5] = SPATIAL_32_BUF[13]; 00238 SPATIAL_LONGITUDE.DATA8[6] = SPATIAL_32_BUF[14]; 00239 SPATIAL_LONGITUDE.DATA8[7] = SPATIAL_32_BUF[15]; //얘들을 모아서. 00240 LONGITUDE = SPATIAL_LONGITUDE.F8 * RADIANS_TO_DEGREES; //double형으로 짠. 라디안이라 고침. 00241 00242 00243 FP8 SPATIAL_HEIGHT; //이 과정은 8바이트로 흩어진 것을 하나의 double형으로 모으는 과정.. 00244 SPATIAL_HEIGHT.DATA8[0] = SPATIAL_32_BUF[16]; 00245 SPATIAL_HEIGHT.DATA8[1] = SPATIAL_32_BUF[17]; 00246 SPATIAL_HEIGHT.DATA8[2] = SPATIAL_32_BUF[18]; 00247 SPATIAL_HEIGHT.DATA8[3] = SPATIAL_32_BUF[19]; 00248 SPATIAL_HEIGHT.DATA8[4] = SPATIAL_32_BUF[20]; 00249 SPATIAL_HEIGHT.DATA8[5] = SPATIAL_32_BUF[21]; 00250 SPATIAL_HEIGHT.DATA8[6] = SPATIAL_32_BUF[22]; 00251 SPATIAL_HEIGHT.DATA8[7] = SPATIAL_32_BUF[23]; //얘들을 모아서. 00252 HEIGHT = SPATIAL_HEIGHT.F8; //double형으로 짠. 라디안이라 고침. 00253 00254 } 00255 } 00256 } 00257 void SPATIAL::SPATIAL_ID_39_Parsing(){ 00258 if(SPATIAL_ID_39_PARSING_BOOL == true){ 00259 SPATIAL_ID_39_PARSING_BOOL = false; 00260 if(CRC_INPUT == calculate_crc16(SPATIAL_39_BUF,12)){ 00261 FP4 SPATIAL_ROLL; //이 과정은 4바이트로 흩어진 것을 하나의 float형으로 모으는 과정.. 00262 SPATIAL_ROLL.DATA4[0] = SPATIAL_39_BUF[0]; 00263 SPATIAL_ROLL.DATA4[1] = SPATIAL_39_BUF[1]; 00264 SPATIAL_ROLL.DATA4[2] = SPATIAL_39_BUF[2]; 00265 SPATIAL_ROLL.DATA4[3] = SPATIAL_39_BUF[3]; 00266 ROLL = SPATIAL_ROLL.F4 * RADIANS_TO_DEGREES; //float형으로 짠. 단위는 역시 rad라서 고침. 00267 00268 FP4 SPATIAL_PITCH; //이 과정은 4바이트로 흩어진 것을 하나의 float형으로 모으는 과정.. 00269 SPATIAL_PITCH.DATA4[0] = SPATIAL_39_BUF[4]; 00270 SPATIAL_PITCH.DATA4[1] = SPATIAL_39_BUF[5]; 00271 SPATIAL_PITCH.DATA4[2] = SPATIAL_39_BUF[6]; 00272 SPATIAL_PITCH.DATA4[3] = SPATIAL_39_BUF[7]; 00273 PITCH = SPATIAL_PITCH.F4 * RADIANS_TO_DEGREES; //float형으로 짠. 단위는 역시 rad라서 고침. 00274 00275 FP4 SPATIAL_YAW; //이 과정은 4바이트로 흩어진 것을 하나의 float형으로 모으는 과정.. 00276 SPATIAL_YAW.DATA4[0] = SPATIAL_39_BUF[8]; 00277 SPATIAL_YAW.DATA4[1] = SPATIAL_39_BUF[9]; 00278 SPATIAL_YAW.DATA4[2] = SPATIAL_39_BUF[10]; 00279 SPATIAL_YAW.DATA4[3] = SPATIAL_39_BUF[11]; 00280 YAW = SPATIAL_YAW.F4 * RADIANS_TO_DEGREES; //float형으로 짠. 단위는 역시 rad라서 고침. 00281 YAW += DECLINATION_ANGLE; 00282 00283 if(YAW > 360) YAW -= 360; 00284 else if(YAW < 0) YAW += 360; 00285 } 00286 } 00287 } 00288 void SPATIAL::SPATIAL_ID_42_Parsing(){ 00289 if(SPATIAL_ID_42_PARSING_BOOL == true){ 00290 SPATIAL_ID_42_PARSING_BOOL = false; 00291 if(CRC_INPUT == calculate_crc16(SPATIAL_42_BUF,12)){ 00292 FP4 SPATIAL_ROLL_RATE; //이 과정은 4바이트로 흩어진 것을 하나의 float형으로 모으는 과정.. 00293 SPATIAL_ROLL_RATE.DATA4[0] = SPATIAL_42_BUF[0]; 00294 SPATIAL_ROLL_RATE.DATA4[1] = SPATIAL_42_BUF[1]; 00295 SPATIAL_ROLL_RATE.DATA4[2] = SPATIAL_42_BUF[2]; 00296 SPATIAL_ROLL_RATE.DATA4[3] = SPATIAL_42_BUF[3]; 00297 ROLL_RATE = SPATIAL_ROLL_RATE.F4 * RADIANS_TO_DEGREES; //float형으로 짠. 단위는 역시 rad라서 고침. 00298 00299 FP4 SPATIAL_PITCH_RATE; //이 과정은 4바이트로 흩어진 것을 하나의 float형으로 모으는 과정.. 00300 SPATIAL_PITCH_RATE.DATA4[0] = SPATIAL_42_BUF[4]; 00301 SPATIAL_PITCH_RATE.DATA4[1] = SPATIAL_42_BUF[5]; 00302 SPATIAL_PITCH_RATE.DATA4[2] = SPATIAL_42_BUF[6]; 00303 SPATIAL_PITCH_RATE.DATA4[3] = SPATIAL_42_BUF[7]; 00304 PITCH_RATE = SPATIAL_PITCH_RATE.F4 * RADIANS_TO_DEGREES; //float형으로 짠. 단위는 역시 rad라서 고침. 00305 00306 FP4 SPATIAL_YAW_RATE; //이 과정은 4바이트로 흩어진 것을 하나의 float형으로 모으는 과정.. 00307 SPATIAL_YAW_RATE.DATA4[0] = SPATIAL_42_BUF[8]; 00308 SPATIAL_YAW_RATE.DATA4[1] = SPATIAL_42_BUF[9]; 00309 SPATIAL_YAW_RATE.DATA4[2] = SPATIAL_42_BUF[10]; 00310 SPATIAL_YAW_RATE.DATA4[3] = SPATIAL_42_BUF[11]; 00311 YAW_RATE = SPATIAL_YAW_RATE.F4 * RADIANS_TO_DEGREES; //float형으로 짠. 단위는 역시 rad라서 고침. 00312 } 00313 } 00314 } 00315 00316 bool SPATIAL::SPATIAL_STABLE(){ 00317 if(SENSOR_WAIT_BOOL == true){ 00318 if( (Vx<0.1) & (Vx>-0.1) & (Vy<0.1) & (Vy>-0.1) & (Vz<0.1) & (Vz>-0.1) ){ 00319 SENSOR_STABLE_CNT ++; 00320 }else{ 00321 SENSOR_STABLE_CNT = 0; 00322 } 00323 if( SENSOR_STABLE_CNT > 2000){ 00324 SENSOR_STABLE_OK = true; 00325 } 00326 /////////////////////////////// 00327 if(SENSOR_STABLE_OK == true){ 00328 return true; 00329 }else{ 00330 return false; 00331 } 00332 }else{ 00333 return true; 00334 } 00335 } 00336
Generated on Sat Jul 16 2022 14:19:21 by
1.7.2