Colin Stearns
/
qcControl
QC Control software
Fork of dgps by
handle/mavcontrol.cpp
- Committer:
- dylanembed123
- Date:
- 2014-04-26
- Revision:
- 52:b4dddb28dffa
- Parent:
- 26:06f1c9d70e9f
File content as of revision 52:b4dddb28dffa:
#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 ",output[i]); } USB::getSerial().printf("\n"); delete output; }