| 0,0 → 1,87 |
|---|
| /*fc_comm.c allows encoding and decoding of the serial protocol, the |
| FlightCtrl uses. |
| The encode and decode algorithms are part of the FlightCtrl software and |
| are therefore written by the MikroKopter team. */ |
| |
| //Include header files, which provide resources, fc_comm.c needs: |
| #include "pc_serial_port.h" |
| |
| #include "fc_comm.h" |
| |
| //Global variables: |
| char tx_buffer[150]; //Transmit Buffer (SendOutData places coded frame here) |
| char rx_buffer[150]; //Receive Buffer (Decode64 reads this buffer) |
| |
| |
| |
| /*AddCRC calculates two byte sized CRC checksums for the encoded frame and |
| adds them to the end of the data-frame. Additionally it adds an \r escape |
| sequence to the end of the frame (defined by the Mikrokopter-Protocoll) */ |
| void AddCRC(unsigned int frame_length) { //length of #,adr,cmd,data |
| |
| unsigned int tmpCRC = 0; |
| unsigned int i; |
| |
| for (i=0; i < frame_length;i++) |
| { |
| tmpCRC += tx_buffer[i]; |
| } |
| |
| tmpCRC %= 4096; |
| tx_buffer[i++] = '=' + tmpCRC / 64; |
| tx_buffer[i++] = '=' + tmpCRC % 64; |
| tx_buffer[i++] = '\r'; |
| |
| } |
| |
| |
| void SendOutData(unsigned char cmd,unsigned char addr, unsigned char *snd, unsigned char len) { |
| |
| unsigned int pt = 0; |
| unsigned char a,b,c; |
| unsigned char ptr = 0; |
| |
| tx_buffer[pt++] = '#'; // Start-Byte |
| tx_buffer[pt++] = 'a' + addr; // Adress |
| tx_buffer[pt++] = cmd; // Command |
| while(len) |
| { |
| if(len) { a = snd[ptr++]; len--;} else a = 0; |
| if(len) { b = snd[ptr++]; len--;} else b = 0; |
| if(len) { c = snd[ptr++]; len--;} else c = 0; |
| tx_buffer[pt++] = '=' + (a >> 2); |
| tx_buffer[pt++] = '=' + (((a & 0x03) << 4) | ((b & 0xf0) >> 4)); |
| tx_buffer[pt++] = '=' + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6)); |
| tx_buffer[pt++] = '=' + ( c & 0x3f); |
| } |
| |
| AddCRC(pt); |
| |
| sendStringToCom(tx_buffer, pt+3); //whole frame length is pt+3 |
| } //#,adr,cmd,data ; crc1,crc2,\r |
| |
| |
| void Decode64(unsigned char *ptrOut, unsigned char len, unsigned char ptrIn,unsigned char max) { |
| |
| unsigned char a,b,c,d; |
| unsigned char ptr = 0; |
| unsigned char x,y,z; |
| |
| while(len) |
| { |
| a = rx_buffer[ptrIn++] - '='; |
| b = rx_buffer[ptrIn++] - '='; |
| c = rx_buffer[ptrIn++] - '='; |
| d = rx_buffer[ptrIn++] - '='; |
| |
| if(ptrIn > max - 2) break; // dont process more data than recieved |
| |
| x = (a << 2) | (b >> 4); |
| y = ((b & 0x0f) << 4) | (c >> 2); |
| z = ((c & 0x03) << 6) | d; |
| |
| if(len--) ptrOut[ptr++] = x; else break; |
| if(len--) ptrOut[ptr++] = y; else break; |
| if(len--) ptrOut[ptr++] = z; else break; |
| } |
| } |