Subversion Repositories Projects

Rev

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
}