Details | Last modification | View Log | RSS feed
| Rev | Author | Line No. | Line |
|---|---|---|---|
| 178 | Alpin | 1 | /*fc_comm.c allows encoding and decoding of the serial protocol, the |
| 2 | FlightCtrl uses. |
||
| 3 | The encode and decode algorithms are part of the FlightCtrl software and |
||
| 4 | are therefore written by the MikroKopter team. */ |
||
| 5 | |||
| 6 | //Include header files, which provide resources, fc_comm.c needs: |
||
| 7 | #include "pc_serial_port.h" |
||
| 8 | |||
| 9 | #include "fc_comm.h" |
||
| 10 | |||
| 11 | //Global variables: |
||
| 12 | char tx_buffer[150]; //Transmit Buffer (SendOutData places coded frame here) |
||
| 13 | char rx_buffer[150]; //Receive Buffer (Decode64 reads this buffer) |
||
| 14 | |||
| 15 | |||
| 16 | |||
| 17 | /*AddCRC calculates two byte sized CRC checksums for the encoded frame and |
||
| 18 | adds them to the end of the data-frame. Additionally it adds an \r escape |
||
| 19 | sequence to the end of the frame (defined by the Mikrokopter-Protocoll) */ |
||
| 20 | void AddCRC(unsigned int frame_length) { //length of #,adr,cmd,data |
||
| 21 | |||
| 22 | unsigned int tmpCRC = 0; |
||
| 23 | unsigned int i; |
||
| 24 | |||
| 25 | for (i=0; i < frame_length;i++) |
||
| 26 | { |
||
| 27 | tmpCRC += tx_buffer[i]; |
||
| 28 | } |
||
| 29 | |||
| 30 | tmpCRC %= 4096; |
||
| 31 | tx_buffer[i++] = '=' + tmpCRC / 64; |
||
| 32 | tx_buffer[i++] = '=' + tmpCRC % 64; |
||
| 33 | tx_buffer[i++] = '\r'; |
||
| 34 | |||
| 35 | } |
||
| 36 | |||
| 37 | |||
| 38 | void SendOutData(unsigned char cmd,unsigned char addr, unsigned char *snd, unsigned char len) { |
||
| 39 | |||
| 40 | unsigned int pt = 0; |
||
| 41 | unsigned char a,b,c; |
||
| 42 | unsigned char ptr = 0; |
||
| 43 | |||
| 44 | tx_buffer[pt++] = '#'; // Start-Byte |
||
| 45 | tx_buffer[pt++] = 'a' + addr; // Adress |
||
| 46 | tx_buffer[pt++] = cmd; // Command |
||
| 47 | while(len) |
||
| 48 | { |
||
| 49 | if(len) { a = snd[ptr++]; len--;} else a = 0; |
||
| 50 | if(len) { b = snd[ptr++]; len--;} else b = 0; |
||
| 51 | if(len) { c = snd[ptr++]; len--;} else c = 0; |
||
| 52 | tx_buffer[pt++] = '=' + (a >> 2); |
||
| 53 | tx_buffer[pt++] = '=' + (((a & 0x03) << 4) | ((b & 0xf0) >> 4)); |
||
| 54 | tx_buffer[pt++] = '=' + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6)); |
||
| 55 | tx_buffer[pt++] = '=' + ( c & 0x3f); |
||
| 56 | } |
||
| 57 | |||
| 58 | AddCRC(pt); |
||
| 59 | |||
| 60 | sendStringToCom(tx_buffer, pt+3); //whole frame length is pt+3 |
||
| 61 | } //#,adr,cmd,data ; crc1,crc2,\r |
||
| 62 | |||
| 63 | |||
| 64 | void Decode64(unsigned char *ptrOut, unsigned char len, unsigned char ptrIn,unsigned char max) { |
||
| 65 | |||
| 66 | unsigned char a,b,c,d; |
||
| 67 | unsigned char ptr = 0; |
||
| 68 | unsigned char x,y,z; |
||
| 69 | |||
| 70 | while(len) |
||
| 71 | { |
||
| 72 | a = rx_buffer[ptrIn++] - '='; |
||
| 73 | b = rx_buffer[ptrIn++] - '='; |
||
| 74 | c = rx_buffer[ptrIn++] - '='; |
||
| 75 | d = rx_buffer[ptrIn++] - '='; |
||
| 76 | |||
| 77 | if(ptrIn > max - 2) break; // dont process more data than recieved |
||
| 78 | |||
| 79 | x = (a << 2) | (b >> 4); |
||
| 80 | y = ((b & 0x0f) << 4) | (c >> 2); |
||
| 81 | z = ((c & 0x03) << 6) | d; |
||
| 82 | |||
| 83 | if(len--) ptrOut[ptr++] = x; else break; |
||
| 84 | if(len--) ptrOut[ptr++] = y; else break; |
||
| 85 | if(len--) ptrOut[ptr++] = z; else break; |
||
| 86 | } |
||
| 87 | } |