File content as of revision 26:06f1c9d70e9f:

#define X25_INIT_CRC 0xffff
#define X25_VALIDATE_CRC 0xf0b8
#include "mavcontrol.h"
#include "usb.h"
#define MAVLINK_CRC_EXTRA 1

Serial* Mav::mav=NULL;
Serial& Mav::getSerial(){
    if(mav==NULL){
        // Init Serial USB
        mav=new Serial(MAVPINTX,MAVPINRX);
        mav->baud(MAVBAUD);
    }
    return *mav;
}

static inline void crc_accumulate(uint8_t data, uint16_t *crcAccum){
        /*Accumulate one byte of data into the CRC*/
        uint8_t tmp;

        tmp = data ^ (uint8_t)(*crcAccum &0xff);
        tmp ^= (tmp<<4);
        *crcAccum = (*crcAccum>>8) ^ (tmp<<8) ^ (tmp <<3) ^ (tmp>>4);
}

static inline void crc_init(uint16_t* crcAccum){
        *crcAccum = X25_INIT_CRC;
}

static inline uint16_t crc_calculate(const uint8_t* pBuffer, uint16_t length){
        uint16_t crcTmp;
        crc_init(&crcTmp);
    while (length--) {
                crc_accumulate(*pBuffer++, &crcTmp);
        }
        return crcTmp;
        //return 5;
}

static int seqcount=0;
int crcExtraCount=50;
char* Mav::generatePacket(int messageID,char* payload,int length,int* outLength){
    char* output=new char[MAVMAXSIZE+1];
    output[0]=0xFE;
    output[1]=(0xFF&length);
    output[2]=(0xFF&(seqcount++));
    output[3]=2;// Sending system ID
    output[4]=1;// Sending system component ID
    output[5]=0xFF&messageID;
    for(int i=0;i<length;i++){output[6+i]=payload[i];}
    int ofs=6+length;
    // Determine checksum
    uint16_t checksum = crc_calculate((uint8_t*)&output[1], length + 5);
    crc_accumulate(crcExtraCount, &checksum);
    output[ofs+0]=(0xFF&checksum);
    output[ofs+1]=(0xFF&(checksum>>0x08));
    if(outLength!=NULL){*outLength=ofs+2;}
    return output;
}

void Mav::sendOutput(int messageID,char* payload,int length){
    int outLength;
    char* output=generatePacket(messageID,payload,length,&outLength);
    for(int i=0;i<outLength;i++){
        getSerial().putc(output[i]);
        USB::getSerial().printf("+ %x\n",output[i]);
    }
    delete output;
}