Subversion Repositories Projects

/*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;
        }
}