Colin Stearns / Mbed 2 deprecated qcControl

QC Control software

Dependencies:   mbed

Fork of dgps by Colin Stearns

handle/mavcontrol.cpp@66:5d43988d100c, 2014-05-05 (annotated)

Committer:
dylanembed123
Date:
Mon May 05 13:20:35 2014 +0000
Revision:
66:5d43988d100c
Parent:
52:b4dddb28dffa 
Final Project;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
dylanembed123 21:c546eab07e28 1
dylanembed123 21:c546eab07e28 2 #define X25_INIT_CRC 0xffff
dylanembed123 21:c546eab07e28 3 #define X25_VALIDATE_CRC 0xf0b8
dylanembed123 21:c546eab07e28 4 #include "mavcontrol.h"
dylanembed123 21:c546eab07e28 5 #include "usb.h"
dylanembed123 21:c546eab07e28 6 #define MAVLINK_CRC_EXTRA 1
dylanembed123 21:c546eab07e28 7
dylanembed123 21:c546eab07e28 8 Serial* Mav::mav=NULL;
dylanembed123 21:c546eab07e28 9 Serial& Mav::getSerial(){
dylanembed123 21:c546eab07e28 10 if(mav==NULL){
dylanembed123 21:c546eab07e28 11 // Init Serial USB
dylanembed123 21:c546eab07e28 12 mav=new Serial(MAVPINTX,MAVPINRX);
dylanembed123 21:c546eab07e28 13 mav->baud(MAVBAUD);
dylanembed123 21:c546eab07e28 14 }
dylanembed123 21:c546eab07e28 15 return *mav;
dylanembed123 21:c546eab07e28 16 }
dylanembed123 21:c546eab07e28 17
dylanembed123 21:c546eab07e28 18 static inline void crc_accumulate(uint8_t data, uint16_t *crcAccum){
dylanembed123 21:c546eab07e28 19 /*Accumulate one byte of data into the CRC*/
dylanembed123 21:c546eab07e28 20 uint8_t tmp;
dylanembed123 21:c546eab07e28 21
dylanembed123 21:c546eab07e28 22 tmp = data ^ (uint8_t)(*crcAccum &0xff);
dylanembed123 21:c546eab07e28 23 tmp ^= (tmp<<4);
dylanembed123 21:c546eab07e28 24 *crcAccum = (*crcAccum>>8) ^ (tmp<<8) ^ (tmp <<3) ^ (tmp>>4);
dylanembed123 21:c546eab07e28 25 }
dylanembed123 21:c546eab07e28 26
dylanembed123 21:c546eab07e28 27 static inline void crc_init(uint16_t* crcAccum){
dylanembed123 21:c546eab07e28 28 *crcAccum = X25_INIT_CRC;
dylanembed123 21:c546eab07e28 29 }
dylanembed123 21:c546eab07e28 30
dylanembed123 21:c546eab07e28 31 static inline uint16_t crc_calculate(const uint8_t* pBuffer, uint16_t length){
dylanembed123 21:c546eab07e28 32 uint16_t crcTmp;
dylanembed123 21:c546eab07e28 33 crc_init(&crcTmp);
dylanembed123 21:c546eab07e28 34 while (length--) {
dylanembed123 21:c546eab07e28 35 crc_accumulate(*pBuffer++, &crcTmp);
dylanembed123 21:c546eab07e28 36 }
dylanembed123 21:c546eab07e28 37 return crcTmp;
dylanembed123 21:c546eab07e28 38 //return 5;
dylanembed123 21:c546eab07e28 39 }
dylanembed123 21:c546eab07e28 40
dylanembed123 21:c546eab07e28 41 static int seqcount=0;
dylanembed123 21:c546eab07e28 42 int crcExtraCount=50;
dylanembed123 21:c546eab07e28 43 char* Mav::generatePacket(int messageID,char* payload,int length,int* outLength){
dylanembed123 21:c546eab07e28 44 char* output=new char[MAVMAXSIZE+1];
dylanembed123 21:c546eab07e28 45 output[0]=0xFE;
dylanembed123 21:c546eab07e28 46 output[1]=(0xFF&length);
dylanembed123 21:c546eab07e28 47 output[2]=(0xFF&(seqcount++));
dylanembed123 21:c546eab07e28 48 output[3]=2;// Sending system ID
dylanembed123 21:c546eab07e28 49 output[4]=1;// Sending system component ID
dylanembed123 21:c546eab07e28 50 output[5]=0xFF&messageID;
dylanembed123 21:c546eab07e28 51 for(int i=0;i<length;i++){output[6+i]=payload[i];}
dylanembed123 21:c546eab07e28 52 int ofs=6+length;
dylanembed123 21:c546eab07e28 53 // Determine checksum
dylanembed123 21:c546eab07e28 54 uint16_t checksum = crc_calculate((uint8_t*)&output[1], length + 5);
dylanembed123 21:c546eab07e28 55 crc_accumulate(crcExtraCount, &checksum);
dylanembed123 21:c546eab07e28 56 output[ofs+0]=(0xFF&checksum);
dylanembed123 21:c546eab07e28 57 output[ofs+1]=(0xFF&(checksum>>0x08));
dylanembed123 21:c546eab07e28 58 if(outLength!=NULL){*outLength=ofs+2;}
dylanembed123 21:c546eab07e28 59 return output;
dylanembed123 21:c546eab07e28 60 }
dylanembed123 21:c546eab07e28 61
dylanembed123 21:c546eab07e28 62 void Mav::sendOutput(int messageID,char* payload,int length){
dylanembed123 21:c546eab07e28 63 int outLength;
dylanembed123 21:c546eab07e28 64 char* output=generatePacket(messageID,payload,length,&outLength);
dylanembed123 21:c546eab07e28 65 for(int i=0;i<outLength;i++){
dylanembed123 21:c546eab07e28 66 getSerial().putc(output[i]);
dylanembed123 52:b4dddb28dffa 67 USB::getSerial().printf("+%x ",output[i]);
dylanembed123 21:c546eab07e28 68 }
dylanembed123 52:b4dddb28dffa 69 USB::getSerial().printf("\n");
dylanembed123 21:c546eab07e28 70 delete output;
dylanembed123 21:c546eab07e28 71 }