Details | Last modification | View Log | RSS feed
| Rev | Author | Line No. | Line |
|---|---|---|---|
| 2105 | - | 1 | #include "main.h" |
| 2 | #include "uart.h" |
||
| 3 | |||
| 4 | int uart0_computer = -1; |
||
| 5 | int uart1_kopter = -1; |
||
| 6 | int uart2_flow = -1; |
||
| 7 | int uart3_gps = -1; |
||
| 8 | |||
| 9 | //>> Storing Chars one by one |
||
| 10 | //------------------------------------------------------------------------------------------------------ |
||
| 11 | u8 get_char(serial_data_struct* pdata_package){ |
||
| 12 | if(pdata_package->count--) |
||
| 13 | { |
||
| 14 | pdata_package->c = pdata_package->txrxdata[pdata_package->position++]; |
||
| 15 | return(1); |
||
| 16 | }else{return(0);} |
||
| 17 | } |
||
| 18 | |||
| 19 | //>> Searching for initiation-character and storing message |
||
| 20 | //------------------------------------------------------------------------------------------------------ |
||
| 21 | u8 collect_data(serial_data_struct* pdata_package){ |
||
| 22 | if(pdata_package->c == '#') |
||
| 23 | { |
||
| 24 | pdata_package->collecting = 1; |
||
| 25 | } |
||
| 26 | if(pdata_package->collecting) |
||
| 27 | { |
||
| 28 | pdata_package->collecting_data[pdata_package->position_package++] = pdata_package->c; |
||
| 29 | if(pdata_package->c == '\r'){ |
||
| 30 | return(0); |
||
| 31 | } |
||
| 32 | } |
||
| 33 | return(1); |
||
| 34 | } |
||
| 35 | |||
| 36 | //>> Searching for initiation-character and storing message (for Flow Control) |
||
| 37 | //------------------------------------------------------------------------------------------------------ |
||
| 38 | u8 collect_data_flow(serial_data_struct* pdata_package){ |
||
| 39 | if(pdata_package->c == 254) |
||
| 40 | { |
||
| 41 | pdata_package->collecting = 1; |
||
| 42 | } |
||
| 43 | if(pdata_package->collecting) |
||
| 44 | { |
||
| 45 | pdata_package->collecting_data[pdata_package->position_package++] = pdata_package->c; |
||
| 46 | if(pdata_package->position_package == pdata_package->length_payload + 8){ |
||
| 47 | return(0); |
||
| 48 | } |
||
| 49 | } |
||
| 50 | return(1); |
||
| 51 | } |
||
| 52 | |||
| 53 | //>> Searching for initiation-character and storing message (for GPS) |
||
| 54 | //------------------------------------------------------------------------------------------------------ |
||
| 55 | u8 collect_data_gps(serial_data_struct* pdata_package){ |
||
| 56 | if(pdata_package->c == '$') |
||
| 57 | { |
||
| 58 | pdata_package->collecting = 1; |
||
| 59 | } |
||
| 60 | if(pdata_package->collecting) |
||
| 61 | { |
||
| 62 | if(pdata_package->c == ','){ |
||
| 63 | pdata_package->position_package_gps++; |
||
| 64 | pdata_package->position_package = 0; |
||
| 65 | }else{ |
||
| 66 | pdata_package->gps_data[pdata_package->position_package_gps][pdata_package->position_package++] = pdata_package->c; |
||
| 67 | } |
||
| 68 | if(pdata_package->c == '\r'){ |
||
| 69 | return(0); |
||
| 70 | } |
||
| 71 | } |
||
| 72 | return(1); |
||
| 73 | } |
||
| 74 | |||
| 75 | float tmpflow = 0.0; |
||
| 76 | //>> Receiving Data from the Optical Flow Module and processing it |
||
| 77 | //------------------------------------------------------------------------------------------------------ |
||
| 78 | void receive_data_from_flow(serial_data_struct* pdata_package_flow){ |
||
| 79 | if (uart2_flow != -1) |
||
| 80 | { |
||
| 81 | int rx_length = read(uart2_flow, (void*)pdata_package_flow->txrxdata, 1024); |
||
| 82 | if (rx_length > 0) |
||
| 83 | { |
||
| 84 | pdata_package_flow->cmdLength = rx_length; |
||
| 85 | |||
| 86 | pdata_package_flow->count = rx_length; |
||
| 87 | pdata_package_flow->position = 0; |
||
| 88 | while(get_char(pdata_package_flow)) |
||
| 89 | { |
||
| 90 | if(!collect_data_flow(pdata_package_flow)) |
||
| 91 | { |
||
| 92 | pdata_package_flow->collecting = 0; |
||
| 93 | pdata_package_flow->dataLength = pdata_package_flow->position_package; |
||
| 94 | pdata_package_flow->position_package = 0; |
||
| 95 | //|----------------------------Process Flow Data----------------------------|// |
||
| 96 | //| |// |
||
| 97 | process_flow_data(pdata_package_flow); |
||
| 98 | //perform_flight_correction(); |
||
| 99 | //| |// |
||
| 100 | //|----------------------------Process Flow Data----------------------------|// |
||
| 101 | memset(pdata_package_flow->collecting_data, 0, 1023); |
||
| 102 | }else{ |
||
| 103 | if(pdata_package_flow->position_package == 6){ |
||
| 104 | pdata_package_flow->length_payload = pdata_package_flow->collecting_data[1]; |
||
| 105 | pdata_package_flow->msg_id = pdata_package_flow->collecting_data[5]; |
||
| 106 | } |
||
| 107 | if(pdata_package_flow->collecting_data[0] != 254){ |
||
| 108 | memset(pdata_package_flow->collecting_data, 0, 1023); |
||
| 109 | memset(pdata_package_flow->txrxdata, 0, 1023); |
||
| 110 | pdata_package_flow->collecting = 0; |
||
| 111 | pdata_package_flow->position_package = 0; |
||
| 112 | pdata_package_flow->msg_id = 0; |
||
| 113 | } |
||
| 114 | } |
||
| 115 | } |
||
| 116 | memset(pdata_package_flow->txrxdata, 0, 1023); |
||
| 117 | |||
| 118 | } |
||
| 119 | }else{ |
||
| 120 | printf("FLOW RX Error\n"); |
||
| 121 | } |
||
| 122 | } |
||
| 123 | |||
| 124 | //>> Receiving Data from the MikroKopter-tool und transmitting it to the kopter |
||
| 125 | //------------------------------------------------------------------------------------------------------ |
||
| 126 | void receive_data_from_computer(serial_data_struct* pdata_package_receive_computer, u8 psendThrough){ |
||
| 127 | if (uart0_computer != -1) |
||
| 128 | { |
||
| 129 | memset(pdata_package_receive_computer->txrxdata, 0, 1023); |
||
| 130 | int rx_length = read(uart0_computer, (void*)pdata_package_receive_computer->txrxdata, 256); //Filestream, buffer to store in, number of bytes to read (max) |
||
| 131 | if (rx_length > 0) |
||
| 132 | { |
||
| 133 | pdata_package_receive_computer->cmdLength = rx_length; |
||
| 134 | transmit_data(TO_KOPTER, pdata_package_receive_computer); |
||
| 135 | |||
| 136 | if(!psendThrough){ |
||
| 137 | pdata_package_receive_computer->count = rx_length; |
||
| 138 | pdata_package_receive_computer->position = 0; |
||
| 139 | while(get_char(pdata_package_receive_computer)) |
||
| 140 | { |
||
| 141 | if(!collect_data(pdata_package_receive_computer)) |
||
| 142 | { |
||
| 143 | pdata_package_receive_computer->collecting = 0; |
||
| 144 | pdata_package_receive_computer->dataLength = pdata_package_receive_computer->position_package; |
||
| 145 | pdata_package_receive_computer->position_package = 0; |
||
| 146 | u16 crc = 0; |
||
| 147 | u8 crc1, crc2; |
||
| 148 | for (int i = 0; i < pdata_package_receive_computer->dataLength - 3; i++) |
||
| 149 | { |
||
| 150 | crc += pdata_package_receive_computer->collecting_data[i]; |
||
| 151 | } |
||
| 152 | crc %= 4096; |
||
| 153 | crc1 = '=' + crc / 64; |
||
| 154 | crc2 = '=' + crc % 64; |
||
| 155 | if(crc1 == pdata_package_receive_computer->collecting_data[pdata_package_receive_computer->dataLength - 3] && crc2 == pdata_package_receive_computer->collecting_data[pdata_package_receive_computer->dataLength - 2]) |
||
| 156 | { |
||
| 157 | analyze_data_package(pdata_package_receive_computer); |
||
| 158 | //>> CRC OK |
||
| 159 | pdata_package_receive_computer->position_package = 0; |
||
| 160 | }else{ |
||
| 161 | //>> CRC NOT OK |
||
| 162 | printf("RX Checksum Error\n"); |
||
| 163 | pdata_package_receive_computer->position_package = 0; |
||
| 164 | memset(pdata_package_receive_computer->collecting_data, 0, 1023); |
||
| 165 | } |
||
| 166 | } |
||
| 167 | } |
||
| 168 | } |
||
| 169 | } |
||
| 170 | }else{ |
||
| 171 | printf("RX Error\n"); |
||
| 172 | } |
||
| 173 | } |
||
| 174 | |||
| 175 | //>> Receiving Data from the Kopter and collecting serial frame if needed |
||
| 176 | //------------------------------------------------------------------------------------------------------ |
||
| 177 | void receive_data_from_kopter(serial_data_struct* pdata_package_receive_kopter){ |
||
| 178 | if (uart1_kopter != -1) |
||
| 179 | { |
||
| 180 | memset(pdata_package_receive_kopter->txrxdata, 0, 1023); |
||
| 181 | int rx_length = read(uart1_kopter, (void*)pdata_package_receive_kopter->txrxdata, 256); //Filestream, buffer to store in, number of bytes to read (max) |
||
| 182 | if (rx_length > 0) |
||
| 183 | { |
||
| 184 | pdata_package_receive_kopter->cmdLength = rx_length; |
||
| 185 | pdata_package_receive_kopter->count = rx_length; |
||
| 186 | pdata_package_receive_kopter->position = 0; |
||
| 187 | while(get_char(pdata_package_receive_kopter)) |
||
| 188 | { |
||
| 189 | if(!collect_data(pdata_package_receive_kopter)) |
||
| 190 | { |
||
| 191 | pdata_package_receive_kopter->collecting = 0; |
||
| 192 | pdata_package_receive_kopter->dataLength = pdata_package_receive_kopter->position_package; |
||
| 193 | pdata_package_receive_kopter->position_package = 0; |
||
| 194 | u16 crc = 0; |
||
| 195 | u8 crc1, crc2; |
||
| 196 | for (int i = 0; i < pdata_package_receive_kopter->dataLength - 3; i++) |
||
| 197 | { |
||
| 198 | crc += pdata_package_receive_kopter->collecting_data[i]; |
||
| 199 | } |
||
| 200 | crc %= 4096; |
||
| 201 | crc1 = '=' + crc / 64; |
||
| 202 | crc2 = '=' + crc % 64; |
||
| 203 | if(crc1 == pdata_package_receive_kopter->collecting_data[pdata_package_receive_kopter->dataLength - 3] && crc2 == pdata_package_receive_kopter->collecting_data[pdata_package_receive_kopter->dataLength - 2]) |
||
| 204 | { |
||
| 205 | analyze_data_package(pdata_package_receive_kopter); |
||
| 206 | //>> CRC OK |
||
| 207 | pdata_package_receive_kopter->position_package = 0; |
||
| 208 | }else{ |
||
| 209 | //>> CRC NOT OK |
||
| 210 | printf("RX Checksum Error\n"); |
||
| 211 | pdata_package_receive_kopter->position_package = 0; |
||
| 212 | memset(pdata_package_receive_kopter->collecting_data, 0, 1023); |
||
| 213 | } |
||
| 214 | } |
||
| 215 | } |
||
| 216 | transmit_data(TO_COMPUTER, pdata_package_receive_kopter); |
||
| 217 | } |
||
| 218 | }else{ |
||
| 219 | printf("KOPTER RX Error\n"); |
||
| 220 | } |
||
| 221 | } |
||
| 222 | |||
| 223 | //>> Receiving Data from the Optical Flow Module and processing it |
||
| 224 | //------------------------------------------------------------------------------------------------------ |
||
| 225 | void receive_data_from_gps(serial_data_struct* pdata_package_gps){ |
||
| 226 | if (uart3_gps != -1) |
||
| 227 | { |
||
| 228 | int rx_length = read(uart3_gps, (void*)pdata_package_gps->txrxdata, 1024); |
||
| 229 | if (rx_length > 0) |
||
| 230 | { |
||
| 231 | pdata_package_gps->cmdLength = rx_length; |
||
| 232 | pdata_package_gps->count = rx_length; |
||
| 233 | pdata_package_gps->position = 0; |
||
| 234 | while(get_char(pdata_package_gps)) |
||
| 235 | { |
||
| 236 | if(!collect_data_gps(pdata_package_gps)) |
||
| 237 | { |
||
| 238 | pdata_package_gps->collecting = 0; |
||
| 239 | pdata_package_gps->dataLength = pdata_package_gps->position_package_gps; |
||
| 240 | pdata_package_gps->position_package = 0; |
||
| 241 | pdata_package_gps->position_package_gps = 0; |
||
| 242 | //|----------------------------Process Flow Data----------------------------|// |
||
| 243 | //| |
||
| 244 | for (int i = 0; i < pdata_package_gps->dataLength; ++i) |
||
| 245 | { |
||
| 246 | process_gps_data(pdata_package_gps); |
||
| 247 | pdata_package_gps->position_package++; |
||
| 248 | } |
||
| 249 | pdata_package_gps->position_package = 0; |
||
| 250 | |||
| 251 | //| |// |
||
| 252 | //|----------------------------Process Flow Data----------------------------|// |
||
| 253 | memset(pdata_package_gps->gps_data, 0, 30*20*sizeof(pdata_package_gps->gps_data[0][0])); |
||
| 254 | } |
||
| 255 | } |
||
| 256 | memset(pdata_package_gps->txrxdata, 0, 1023); } |
||
| 257 | }else{ |
||
| 258 | printf("GPS RX Error\n"); |
||
| 259 | } |
||
| 260 | } |
||
| 261 | |||
| 262 | |||
| 263 | //>> Transmitting data |
||
| 264 | //------------------------------------------------------------------------------------------------------ |
||
| 265 | void transmit_data(int uart_filestream, serial_data_struct* pdata_package_transmit){ |
||
| 266 | if (uart_filestream == 1) |
||
| 267 | { |
||
| 268 | uart_filestream = uart0_computer; |
||
| 269 | /* |
||
| 270 | printf("TO COMPUTER\n"); |
||
| 271 | printf("%s\n", pdata_package_transmit->txrxdata); |
||
| 272 | */ |
||
| 273 | }else if(uart_filestream == 2) |
||
| 274 | { |
||
| 275 | uart_filestream = uart1_kopter; |
||
| 276 | /* |
||
| 277 | printf("TO KOPTER\n"); |
||
| 278 | printf("%s\n", pdata_package_transmit->txrxdata); |
||
| 279 | |||
| 280 | for (int i = 0; i < pdata_package_transmit->cmdLength; ++i) |
||
| 281 | { |
||
| 282 | printf("%d ", pdata_package_transmit->txrxdata[i]); |
||
| 283 | |||
| 284 | } |
||
| 285 | printf("\n"); |
||
| 286 | */ |
||
| 287 | }else if(uart_filestream == 3){ |
||
| 288 | uart_filestream = uart2_flow; |
||
| 289 | } |
||
| 290 | if (uart_filestream != -1) |
||
| 291 | { |
||
| 292 | int count = write(uart_filestream, pdata_package_transmit->txrxdata, pdata_package_transmit->cmdLength); |
||
| 293 | if (count < 0) |
||
| 294 | { |
||
| 295 | //printf("UART TX error: %d\n", uart_filestream); |
||
| 296 | } |
||
| 297 | } |
||
| 298 | } |
||
| 299 | |||
| 300 | //>> Adding address and command ID, encoding and checksum |
||
| 301 | //------------------------------------------------------------------------------------------------------ |
||
| 302 | void create_serial_frame(u8 address, u8 cmdID, u16 cmdLength, serial_data_struct* pdata_package_create){ |
||
| 303 | pdata_package_create->txrxdata[0] = '\r'; |
||
| 304 | pdata_package_create->txrxdata[1] = '#'; |
||
| 305 | pdata_package_create->txrxdata[2] = address + 'a'; |
||
| 306 | pdata_package_create->txrxdata[3] = cmdID; |
||
| 307 | //encode data |
||
| 308 | u8 a,b,c; |
||
| 309 | int counter = 0; |
||
| 310 | int u = 4; |
||
| 311 | while(cmdLength){ |
||
| 312 | if(cmdLength) |
||
| 313 | { |
||
| 314 | cmdLength--; |
||
| 315 | a = pdata_package_create->data[counter++]; |
||
| 316 | }else{a = 0; counter++;}; |
||
| 317 | if(cmdLength) |
||
| 318 | { |
||
| 319 | cmdLength--; |
||
| 320 | b = pdata_package_create->data[counter++]; |
||
| 321 | }else{b = 0; counter++;}; |
||
| 322 | if(cmdLength) |
||
| 323 | { |
||
| 324 | cmdLength--; |
||
| 325 | c = pdata_package_create->data[counter++]; |
||
| 326 | }else{c = 0; counter++;}; |
||
| 327 | pdata_package_create->txrxdata[u++] = '=' + (a >> 2); |
||
| 328 | pdata_package_create->txrxdata[u++] = '=' + (((a & 0x03) << 4) | ((b & 0xf0) >> 4)); |
||
| 329 | pdata_package_create->txrxdata[u++] = '=' + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6)); |
||
| 330 | pdata_package_create->txrxdata[u++] = '=' + (c & 0x3f); |
||
| 331 | } |
||
| 332 | //add Checksum |
||
| 333 | u16 crc = 0; |
||
| 334 | u8 crc1, crc2; |
||
| 335 | for (int i = 1; i < u; i++) |
||
| 336 | { |
||
| 337 | crc += pdata_package_create->txrxdata[i]; |
||
| 338 | } |
||
| 339 | crc %= 4096; |
||
| 340 | crc1 = '=' + crc / 64; |
||
| 341 | crc2 = '=' + crc % 64; |
||
| 342 | pdata_package_create->txrxdata[u++] = crc1; |
||
| 343 | pdata_package_create->txrxdata[u++] = crc2; |
||
| 344 | pdata_package_create->txrxdata[u++] = '\r'; |
||
| 345 | |||
| 346 | pdata_package_create->cmdLength = u; |
||
| 347 | } |
||
| 348 | |||
| 349 | //>> Decoding Data and retrieving address and cmdID |
||
| 350 | //------------------------------------------------------------------------------------------------------ |
||
| 351 | void collect_serial_frame(serial_data_struct* pdata_package_collect){ |
||
| 352 | pdata_package_collect->position_package++; //first character: # |
||
| 353 | pdata_package_collect->address = pdata_package_collect->collecting_data[pdata_package_collect->position_package++] - 'a'; //address |
||
| 354 | pdata_package_collect->cmdID = pdata_package_collect->collecting_data[pdata_package_collect->position_package++]; //cmdID |
||
| 355 | u8 a, b, c, d; |
||
| 356 | u16 datacpy = 0; |
||
| 357 | while(pdata_package_collect->position_package < pdata_package_collect->dataLength-3){ |
||
| 358 | a = pdata_package_collect->collecting_data[pdata_package_collect->position_package++] - '='; |
||
| 359 | b = pdata_package_collect->collecting_data[pdata_package_collect->position_package++] - '='; |
||
| 360 | c = pdata_package_collect->collecting_data[pdata_package_collect->position_package++] - '='; |
||
| 361 | d = pdata_package_collect->collecting_data[pdata_package_collect->position_package++] - '='; |
||
| 362 | |||
| 363 | pdata_package_collect->data[datacpy++] = (a << 2) | (b >> 4); |
||
| 364 | pdata_package_collect->data[datacpy++] = ((b & 0x0f) << 4) | (c >> 2); |
||
| 365 | pdata_package_collect->data[datacpy++] = ((c & 0x03) << 6) | d; |
||
| 366 | } |
||
| 367 | pdata_package_collect->dataLength = datacpy; |
||
| 368 | pdata_package_collect->position_package = 0; |
||
| 369 | |||
| 370 | /* |
||
| 371 | printf("%s\n", pdata_package_collect->collecting_data); |
||
| 372 | |||
| 373 | for (int i = 0; i < datacpy; ++i) |
||
| 374 | { |
||
| 375 | printf("|%d|: %d ", i, pdata_package_collect->data[i]); |
||
| 376 | } |
||
| 377 | |||
| 378 | printf("%s\n", pdata_package_collect->data); |
||
| 379 | */ |
||
| 380 | } |
||
| 381 | |||
| 382 | |||
| 383 | //>> Initializing serial interface |
||
| 384 | //------------------------------------------------------------------------------------------------------ |
||
| 385 | void uart_init(){ |
||
| 386 | uart0_computer = open(SERIAL_COMPUTER, O_RDWR | O_NOCTTY | O_NDELAY); |
||
| 387 | uart1_kopter = open(SERIAL_KOPTER,O_RDWR | O_NOCTTY | O_NDELAY); |
||
| 388 | uart2_flow = open(SERIAL_FLOW,O_RDWR | O_NOCTTY | O_NDELAY); |
||
| 389 | uart3_gps = open(SERIAL_GPS,O_RDWR | O_NOCTTY | O_NDELAY); |
||
| 390 | if (uart0_computer == -1) |
||
| 391 | { |
||
| 392 | printf("Error - Unable to open UART0 (Computer)!\n"); |
||
| 393 | } |
||
| 394 | if (uart1_kopter == -1) |
||
| 395 | { |
||
| 396 | printf("Error - Unable to open UART1 (Kopter)!\n"); |
||
| 397 | } |
||
| 398 | if (uart2_flow == -1) |
||
| 399 | { |
||
| 400 | printf("Error - Unable to open UART2 (FLOW)!\n"); |
||
| 401 | } |
||
| 402 | if (uart3_gps == -1) |
||
| 403 | { |
||
| 404 | printf("Error - Unable to open UART2 (GPS)!\n"); |
||
| 405 | } |
||
| 406 | //UART Options |
||
| 407 | struct termios options; |
||
| 408 | tcgetattr(uart0_computer, &options); |
||
| 409 | options.c_cflag = B57600 | CS8 | CLOCAL | CREAD; |
||
| 410 | options.c_iflag = IGNPAR; |
||
| 411 | options.c_oflag = 0; |
||
| 412 | options.c_lflag = 0; |
||
| 413 | tcflush(uart0_computer, TCIFLUSH); |
||
| 414 | tcsetattr(uart0_computer, TCSANOW, &options); |
||
| 415 | tcflush(uart1_kopter, TCIFLUSH); |
||
| 416 | tcsetattr(uart1_kopter, TCSANOW, &options); |
||
| 417 | tcflush(uart2_flow, TCIFLUSH); |
||
| 418 | tcsetattr(uart2_flow, TCSANOW, &options); |
||
| 419 | //UART Options |
||
| 420 | |||
| 421 | struct termios options_flow; |
||
| 422 | tcgetattr(uart3_gps, &options_flow); |
||
| 423 | options_flow.c_cflag = B9600 | CS8 | CLOCAL | CREAD; |
||
| 424 | options.c_iflag = IGNPAR; |
||
| 425 | options_flow.c_oflag = 0; |
||
| 426 | options_flow.c_lflag = 0; |
||
| 427 | tcflush(uart3_gps, TCIFLUSH); |
||
| 428 | tcsetattr(uart3_gps, TCSANOW, &options_flow); |
||
| 429 | } |