Diff: mavcontrol.cpp

Revision:

21:c546eab07e28

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mavcontrol.cpp	Mon Apr 21 13:54:55 2014 +0000
@@ -0,0 +1,70 @@
+
+#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;
+}
\ No newline at end of file