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@0:3473b92e991e, 2018-06-12 (annotated)

Committer:
skyyoungsik
Date:
Tue Jun 12 01:05:50 2018 +0000
Revision:
0:3473b92e991e
Child:
1:9530746906b6
lkj;

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 0:3473b92e991e 10 //float 이랑 double 값뽑을때 쓰는거
skyyoungsik 0:3473b92e991e 11 union FP8
skyyoungsik 0:3473b92e991e 12 {
skyyoungsik 0:3473b92e991e 13 char DATA8[8];
skyyoungsik 0:3473b92e991e 14 double F8;
skyyoungsik 0:3473b92e991e 15 };
skyyoungsik 0:3473b92e991e 16 union FP4
skyyoungsik 0:3473b92e991e 17 {
skyyoungsik 0:3473b92e991e 18 char DATA4[4];
skyyoungsik 0:3473b92e991e 19 float F4;
skyyoungsik 0:3473b92e991e 20 };
skyyoungsik 0:3473b92e991e 21
skyyoungsik 0:3473b92e991e 22 uint16_t calculate_crc16(const void *data, uint16_t length)
skyyoungsik 0:3473b92e991e 23 {
skyyoungsik 0:3473b92e991e 24 uint8_t *bytes = (uint8_t *) data;
skyyoungsik 0:3473b92e991e 25 uint16_t crc = 0xFFFF, i;
skyyoungsik 0:3473b92e991e 26 for (i = 0; i < length; i++)
skyyoungsik 0:3473b92e991e 27 {
skyyoungsik 0:3473b92e991e 28 crc = (uint16_t)((crc << 8) ^ crc16_table[(crc >> 8) ^ bytes[i]]);
skyyoungsik 0:3473b92e991e 29 }
skyyoungsik 0:3473b92e991e 30 return crc;
skyyoungsik 0:3473b92e991e 31 }
skyyoungsik 0:3473b92e991e 32
skyyoungsik 0:3473b92e991e 33 void SPATIAL::Init(){
skyyoungsik 0:3473b92e991e 34 SPATIAL_UART.baud(115200);
skyyoungsik 0:3473b92e991e 35 }
skyyoungsik 0:3473b92e991e 36 //이것은 SPATIAL에서 전송되는 각종 데이터를 받는 부분이다.
skyyoungsik 0:3473b92e991e 37 void SPATIAL::SPATIAL_RECEIVE()
skyyoungsik 0:3473b92e991e 38 {
skyyoungsik 0:3473b92e991e 39 while(SPATIAL_UART.readable() > 0)
skyyoungsik 0:3473b92e991e 40 {
skyyoungsik 0:3473b92e991e 41 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 0:3473b92e991e 42 if(SPATIAL_ID_20_BOOL == true){
skyyoungsik 0:3473b92e991e 43 SPATIAL_BUF[SPATIAL_BUF_CNT] = SPATIAL_ID[0];
skyyoungsik 0:3473b92e991e 44 if(SPATIAL_BUF_CNT >= 100){ SPATIAL_ID_20_BOOL = false; SPATIAL_ID_20_PARSING_BOOL = true;}
skyyoungsik 0:3473b92e991e 45 SPATIAL_BUF_CNT ++;
skyyoungsik 0:3473b92e991e 46 }
skyyoungsik 0:3473b92e991e 47 else{
skyyoungsik 0:3473b92e991e 48 if((SPATIAL_ID[3] == 20) & (SPATIAL_ID[2] == 100)) //두번째 데이터가 '패킷넘버', 세번째 데이터가 '데이터랭쓰'임... 별도의 SOF가 없어서 이걸 기준으로함.
skyyoungsik 0:3473b92e991e 49 {
skyyoungsik 0:3473b92e991e 50 //첫 번째 데이터 : head LRC이며 계산식 돌린거랑 첫번째 데이터랑 일치하면 이 배열을 쓸거라는 의미.
skyyoungsik 0:3473b92e991e 51 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 52 {
skyyoungsik 0:3473b92e991e 53 SPATIAL_ID_20_BOOL = true;
skyyoungsik 0:3473b92e991e 54 CRC_INPUT = (uint16_t)SPATIAL_ID[0]<<8 | (uint16_t)SPATIAL_ID[1];
skyyoungsik 0:3473b92e991e 55 SPATIAL_BUF_CNT = 0;
skyyoungsik 0:3473b92e991e 56 }
skyyoungsik 0:3473b92e991e 57 }
skyyoungsik 0:3473b92e991e 58 }
skyyoungsik 0:3473b92e991e 59 }
skyyoungsik 0:3473b92e991e 60 }
skyyoungsik 0:3473b92e991e 61
skyyoungsik 0:3473b92e991e 62 void SPATIAL::SPATIAL_ID_20_Parsing() //데이터 파싱을 실시함 여기서.
skyyoungsik 0:3473b92e991e 63 {
skyyoungsik 0:3473b92e991e 64 if(SPATIAL_ID_20_PARSING_BOOL == true)
skyyoungsik 0:3473b92e991e 65 {
skyyoungsik 0:3473b92e991e 66 SPATIAL_ID_20_PARSING_BOOL = false;
skyyoungsik 0:3473b92e991e 67
skyyoungsik 0:3473b92e991e 68 if(CRC_INPUT == calculate_crc16(SPATIAL_BUF,100)){
skyyoungsik 0:3473b92e991e 69 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];
skyyoungsik 0:3473b92e991e 70 UNIX_TIME /= 1000;
skyyoungsik 0:3473b92e991e 71 FP8 SPATIAL_LATITUDE; //이 과정은 8바이트로 흩어진 것을 하나의 double형으로 모으는 과정..
skyyoungsik 0:3473b92e991e 72 SPATIAL_LATITUDE.DATA8[0] = SPATIAL_BUF[12];
skyyoungsik 0:3473b92e991e 73 SPATIAL_LATITUDE.DATA8[1] = SPATIAL_BUF[13];
skyyoungsik 0:3473b92e991e 74 SPATIAL_LATITUDE.DATA8[2] = SPATIAL_BUF[14];
skyyoungsik 0:3473b92e991e 75 SPATIAL_LATITUDE.DATA8[3] = SPATIAL_BUF[15];
skyyoungsik 0:3473b92e991e 76 SPATIAL_LATITUDE.DATA8[4] = SPATIAL_BUF[16];
skyyoungsik 0:3473b92e991e 77 SPATIAL_LATITUDE.DATA8[5] = SPATIAL_BUF[17];
skyyoungsik 0:3473b92e991e 78 SPATIAL_LATITUDE.DATA8[6] = SPATIAL_BUF[18];
skyyoungsik 0:3473b92e991e 79 SPATIAL_LATITUDE.DATA8[7] = SPATIAL_BUF[19]; //얘들을 모아서.
skyyoungsik 0:3473b92e991e 80 LATITUDE = SPATIAL_LATITUDE.F8 * RADIANS_TO_DEGREES; //double형으로 짠. 라디안이라 고침.
skyyoungsik 0:3473b92e991e 81
skyyoungsik 0:3473b92e991e 82
skyyoungsik 0:3473b92e991e 83
skyyoungsik 0:3473b92e991e 84 FP8 SPATIAL_LONGITUDE; //이 과정은 8바이트로 흩어진 것을 하나의 double형으로 모으는 과정..
skyyoungsik 0:3473b92e991e 85 SPATIAL_LONGITUDE.DATA8[0] = SPATIAL_BUF[20];
skyyoungsik 0:3473b92e991e 86 SPATIAL_LONGITUDE.DATA8[1] = SPATIAL_BUF[21];
skyyoungsik 0:3473b92e991e 87 SPATIAL_LONGITUDE.DATA8[2] = SPATIAL_BUF[22];
skyyoungsik 0:3473b92e991e 88 SPATIAL_LONGITUDE.DATA8[3] = SPATIAL_BUF[23];
skyyoungsik 0:3473b92e991e 89 SPATIAL_LONGITUDE.DATA8[4] = SPATIAL_BUF[24];
skyyoungsik 0:3473b92e991e 90 SPATIAL_LONGITUDE.DATA8[5] = SPATIAL_BUF[25];
skyyoungsik 0:3473b92e991e 91 SPATIAL_LONGITUDE.DATA8[6] = SPATIAL_BUF[26];
skyyoungsik 0:3473b92e991e 92 SPATIAL_LONGITUDE.DATA8[7] = SPATIAL_BUF[27]; //얘들을 모아서.
skyyoungsik 0:3473b92e991e 93 LONGITUDE = SPATIAL_LONGITUDE.F8 * RADIANS_TO_DEGREES; //double형으로 짠. 라디안이라 고침.
skyyoungsik 0:3473b92e991e 94
skyyoungsik 0:3473b92e991e 95
skyyoungsik 0:3473b92e991e 96 FP8 SPATIAL_HEIGHT; //이 과정은 8바이트로 흩어진 것을 하나의 double형으로 모으는 과정..
skyyoungsik 0:3473b92e991e 97 SPATIAL_HEIGHT.DATA8[0] = SPATIAL_BUF[28];
skyyoungsik 0:3473b92e991e 98 SPATIAL_HEIGHT.DATA8[1] = SPATIAL_BUF[29];
skyyoungsik 0:3473b92e991e 99 SPATIAL_HEIGHT.DATA8[2] = SPATIAL_BUF[30];
skyyoungsik 0:3473b92e991e 100 SPATIAL_HEIGHT.DATA8[3] = SPATIAL_BUF[31];
skyyoungsik 0:3473b92e991e 101 SPATIAL_HEIGHT.DATA8[4] = SPATIAL_BUF[32];
skyyoungsik 0:3473b92e991e 102 SPATIAL_HEIGHT.DATA8[5] = SPATIAL_BUF[33];
skyyoungsik 0:3473b92e991e 103 SPATIAL_HEIGHT.DATA8[6] = SPATIAL_BUF[34];
skyyoungsik 0:3473b92e991e 104 SPATIAL_HEIGHT.DATA8[7] = SPATIAL_BUF[35]; //얘들을 모아서.
skyyoungsik 0:3473b92e991e 105 HEIGHT = SPATIAL_HEIGHT.F8 * RADIANS_TO_DEGREES; //double형으로 짠. 라디안이라 고침.
skyyoungsik 0:3473b92e991e 106
skyyoungsik 0:3473b92e991e 107
skyyoungsik 0:3473b92e991e 108 FP4 SPATIAL_Vn; //이 과정은 4바이트로 흩어진 것을 하나의 float형으로 모으는 과정..
skyyoungsik 0:3473b92e991e 109 SPATIAL_Vn.DATA4[0] = SPATIAL_BUF[36];
skyyoungsik 0:3473b92e991e 110 SPATIAL_Vn.DATA4[1] = SPATIAL_BUF[37];
skyyoungsik 0:3473b92e991e 111 SPATIAL_Vn.DATA4[2] = SPATIAL_BUF[38];
skyyoungsik 0:3473b92e991e 112 SPATIAL_Vn.DATA4[3] = SPATIAL_BUF[39];
skyyoungsik 0:3473b92e991e 113 Vn = SPATIAL_Vn.F4; //float형으로 짠. 단위는 역시 rad라서 고침.
skyyoungsik 0:3473b92e991e 114
skyyoungsik 0:3473b92e991e 115
skyyoungsik 0:3473b92e991e 116 FP4 SPATIAL_Ve; //이 과정은 4바이트로 흩어진 것을 하나의 float형으로 모으는 과정..
skyyoungsik 0:3473b92e991e 117 SPATIAL_Ve.DATA4[0] = SPATIAL_BUF[40];
skyyoungsik 0:3473b92e991e 118 SPATIAL_Ve.DATA4[1] = SPATIAL_BUF[41];
skyyoungsik 0:3473b92e991e 119 SPATIAL_Ve.DATA4[2] = SPATIAL_BUF[42];
skyyoungsik 0:3473b92e991e 120 SPATIAL_Ve.DATA4[3] = SPATIAL_BUF[43];
skyyoungsik 0:3473b92e991e 121 Ve = SPATIAL_Ve.F4; //float형으로 짠. 단위는 역시 rad라서 고침.
skyyoungsik 0:3473b92e991e 122
skyyoungsik 0:3473b92e991e 123 FP4 SPATIAL_Vd; //이 과정은 4바이트로 흩어진 것을 하나의 float형으로 모으는 과정..
skyyoungsik 0:3473b92e991e 124 SPATIAL_Vd.DATA4[0] = SPATIAL_BUF[44];
skyyoungsik 0:3473b92e991e 125 SPATIAL_Vd.DATA4[1] = SPATIAL_BUF[45];
skyyoungsik 0:3473b92e991e 126 SPATIAL_Vd.DATA4[2] = SPATIAL_BUF[46];
skyyoungsik 0:3473b92e991e 127 SPATIAL_Vd.DATA4[3] = SPATIAL_BUF[47];
skyyoungsik 0:3473b92e991e 128 Vd = SPATIAL_Vd.F4; //float형으로 짠. 단위는 역시 rad라서 고침.
skyyoungsik 0:3473b92e991e 129
skyyoungsik 0:3473b92e991e 130 Vd = -Vd;
skyyoungsik 0:3473b92e991e 131
skyyoungsik 0:3473b92e991e 132 FP4 SPATIAL_ROLL; //이 과정은 4바이트로 흩어진 것을 하나의 float형으로 모으는 과정..
skyyoungsik 0:3473b92e991e 133 SPATIAL_ROLL.DATA4[0] = SPATIAL_BUF[64];
skyyoungsik 0:3473b92e991e 134 SPATIAL_ROLL.DATA4[1] = SPATIAL_BUF[65];
skyyoungsik 0:3473b92e991e 135 SPATIAL_ROLL.DATA4[2] = SPATIAL_BUF[66];
skyyoungsik 0:3473b92e991e 136 SPATIAL_ROLL.DATA4[3] = SPATIAL_BUF[67];
skyyoungsik 0:3473b92e991e 137 ROLL = SPATIAL_ROLL.F4 * RADIANS_TO_DEGREES; //float형으로 짠. 단위는 역시 rad라서 고침.
skyyoungsik 0:3473b92e991e 138
skyyoungsik 0:3473b92e991e 139 FP4 SPATIAL_PITCH; //이 과정은 4바이트로 흩어진 것을 하나의 float형으로 모으는 과정..
skyyoungsik 0:3473b92e991e 140 SPATIAL_PITCH.DATA4[0] = SPATIAL_BUF[68];
skyyoungsik 0:3473b92e991e 141 SPATIAL_PITCH.DATA4[1] = SPATIAL_BUF[69];
skyyoungsik 0:3473b92e991e 142 SPATIAL_PITCH.DATA4[2] = SPATIAL_BUF[70];
skyyoungsik 0:3473b92e991e 143 SPATIAL_PITCH.DATA4[3] = SPATIAL_BUF[71];
skyyoungsik 0:3473b92e991e 144 PITCH = SPATIAL_PITCH.F4 * RADIANS_TO_DEGREES; //float형으로 짠. 단위는 역시 rad라서 고침.
skyyoungsik 0:3473b92e991e 145
skyyoungsik 0:3473b92e991e 146 FP4 SPATIAL_YAW; //이 과정은 4바이트로 흩어진 것을 하나의 float형으로 모으는 과정..
skyyoungsik 0:3473b92e991e 147 SPATIAL_YAW.DATA4[0] = SPATIAL_BUF[72];
skyyoungsik 0:3473b92e991e 148 SPATIAL_YAW.DATA4[1] = SPATIAL_BUF[73];
skyyoungsik 0:3473b92e991e 149 SPATIAL_YAW.DATA4[2] = SPATIAL_BUF[74];
skyyoungsik 0:3473b92e991e 150 SPATIAL_YAW.DATA4[3] = SPATIAL_BUF[75];
skyyoungsik 0:3473b92e991e 151 YAW = SPATIAL_YAW.F4 * RADIANS_TO_DEGREES; //float형으로 짠. 단위는 역시 rad라서 고침.
skyyoungsik 0:3473b92e991e 152
skyyoungsik 0:3473b92e991e 153
skyyoungsik 0:3473b92e991e 154 FP4 SPATIAL_ROLL_RATE; //이 과정은 4바이트로 흩어진 것을 하나의 float형으로 모으는 과정..
skyyoungsik 0:3473b92e991e 155 SPATIAL_ROLL_RATE.DATA4[0] = SPATIAL_BUF[76];
skyyoungsik 0:3473b92e991e 156 SPATIAL_ROLL_RATE.DATA4[1] = SPATIAL_BUF[77];
skyyoungsik 0:3473b92e991e 157 SPATIAL_ROLL_RATE.DATA4[2] = SPATIAL_BUF[78];
skyyoungsik 0:3473b92e991e 158 SPATIAL_ROLL_RATE.DATA4[3] = SPATIAL_BUF[79];
skyyoungsik 0:3473b92e991e 159 ROLL_RATE = SPATIAL_ROLL_RATE.F4 * RADIANS_TO_DEGREES; //float형으로 짠. 단위는 역시 rad라서 고침.
skyyoungsik 0:3473b92e991e 160
skyyoungsik 0:3473b92e991e 161 FP4 SPATIAL_PITCH_RATE; //이 과정은 4바이트로 흩어진 것을 하나의 float형으로 모으는 과정..
skyyoungsik 0:3473b92e991e 162 SPATIAL_PITCH_RATE.DATA4[0] = SPATIAL_BUF[80];
skyyoungsik 0:3473b92e991e 163 SPATIAL_PITCH_RATE.DATA4[1] = SPATIAL_BUF[81];
skyyoungsik 0:3473b92e991e 164 SPATIAL_PITCH_RATE.DATA4[2] = SPATIAL_BUF[82];
skyyoungsik 0:3473b92e991e 165 SPATIAL_PITCH_RATE.DATA4[3] = SPATIAL_BUF[83];
skyyoungsik 0:3473b92e991e 166 PITCH_RATE = SPATIAL_PITCH_RATE.F4 * RADIANS_TO_DEGREES; //float형으로 짠. 단위는 역시 rad라서 고침.
skyyoungsik 0:3473b92e991e 167
skyyoungsik 0:3473b92e991e 168 FP4 SPATIAL_YAW_RATE; //이 과정은 4바이트로 흩어진 것을 하나의 float형으로 모으는 과정..
skyyoungsik 0:3473b92e991e 169 SPATIAL_YAW_RATE.DATA4[0] = SPATIAL_BUF[84];
skyyoungsik 0:3473b92e991e 170 SPATIAL_YAW_RATE.DATA4[1] = SPATIAL_BUF[85];
skyyoungsik 0:3473b92e991e 171 SPATIAL_YAW_RATE.DATA4[2] = SPATIAL_BUF[86];
skyyoungsik 0:3473b92e991e 172 SPATIAL_YAW_RATE.DATA4[3] = SPATIAL_BUF[87];
skyyoungsik 0:3473b92e991e 173 YAW_RATE = SPATIAL_YAW_RATE.F4 * RADIANS_TO_DEGREES; //float형으로 짠. 단위는 역시 rad라서 고침.
skyyoungsik 0:3473b92e991e 174
skyyoungsik 0:3473b92e991e 175 }
skyyoungsik 0:3473b92e991e 176
skyyoungsik 0:3473b92e991e 177
skyyoungsik 0:3473b92e991e 178 }
skyyoungsik 0:3473b92e991e 179 }
skyyoungsik 0:3473b92e991e 180